#include "stdafx.h"
#include "../MFCD3D11Test.h"
#include "..\\Utility\\UtilDefs.h"
#include "XYZFileIO.h"
#include "..\\Utility\\D3D\\D3DUtilityFunction.h"
#include "..\\Utility\\Math\\SVD.h"

//std library
#include <map>

//Using Eigen library
#include <Eigen/Eigen>
using namespace Eigen;

//Using CImg library
#include "../Utility/CImg.h"
using namespace cimg_library;

const float CXYZFileIO::MINVAL = -999999.0f;

CXYZFileIO::CXYZFileIO(void)
{
	//File handle
	fp = NULL;
	/***************************************************************************************/
	/*member variables for .xyz file*/	
	/***************************************************************************************/
	/*line 1*/
	//string constant 
	m_strConstL1 = NULL;
	/*line 2*/
	m_iSoftwareType = 0;
	m_iMajorVers = 0; m_iMinorVers = 0;	m_iBugVers = 0;
	m_strSoftwareDate = NULL;
	/*line 3*/
	m_iIntensOriginX = 0; m_iIntensOriginY = 0;
	m_iIntensWidth = 0; m_iIntensHeight = 0;
	m_iNBuckets = 0;
	m_uiIntensRange = 0;
	/*line 4*/
	m_iPhaseOriginX = 0; m_iPhaseOriginY = 0;
	m_iPhaseWidth = 0; m_iPhaseHeight = 0;
	/*line 5*/
	m_strComment = NULL;
	/*line 6*/
	m_strPartSerNum = NULL;
	/*line 7*/
	m_strPartNum = NULL;
	/*line 8*/
	m_iSource = 0;
	m_fIntfScaleFactor = 0.0f;
	m_fWavelengthIn = 0.0f;
	m_fNumericAperture = 0.0f;
	m_fObliquityFactor = 0.0f;
	m_fMagnification = 0.0f;
	m_fCameraRes = 0.0f;
	m_iTimeStamp = 0;
	/*line 9*/
	m_iCameraWidth = 0; m_iCameraHeight = 0;
	m_iSystemType = 0;
	m_iSystemBoard = 0;
	m_iSystemSerial = 0;
	m_iInstrumentId = 0;
	m_strObjectiveName = NULL;
	/*line 10*/
	m_iAcquireMode = 0;
	m_iIntensAvgs = 0;
	m_iPZTCal = 0;
	m_iPZTGain = 0;
	m_iPZTGainTolerance = 0;
	m_iAGC = 0;
	m_fTargetRange = 0.0f;
	m_fLightLevel = 0.0f;
	m_iMinMod = 0;
	m_iMinModPts = 0;
	/*line 11*/
	m_iPhaseRes = 0;
	m_iPhaseAvgs = 0;
	m_iMinimumAreaSize = 0;
	m_iDisconAction = 0;
	m_fDisconFilter = 0;
	m_iConnectionOrder = 0;
	m_iRemoveTiltBias = 0;
	m_iDataSign = 0;
	m_iCodeVType = 0;
	/*line 12*/
	m_iSubtractSysErr = 0;
	m_strSysErrFile = NULL;
	/*line 13*/
	m_fRefractiveIndex = 0.0f;
	m_fPartThickness = 0.0f;
	/*line 14*/
	m_strZoomDesc = NULL;
	/***************************************************************************************/
	//height map data
	m_arrHeightMapData = NULL;
	m_iHMDataWidth = -1;
	m_iHMDataHeight = -1;
	m_fHeightRange = 0.0f;

	m_bDataReady = false;
	m_arrVertices.clear();

	//width, height
	m_fSampleWidth = 140;
	m_fSampleHeight = 110;
	
	//normal window
	m_iNormalWndSize = 3;
	m_iCurNormalEstiMethod = USEPCANORMAL;
	//m_iCurNormalEstiMethod = NORMALESTIMATEMETHOD::USEWEIGHTEDPCA_GAUSSIAN;
	//m_i2DNDFImgDim = 1025;
	m_i2DNDFImgDim = 513;
	//m_i2DNDFImgDim = 257;
	//m_i2DNDFImgDim = 129;
	//m_i2DNDFImgDim = 65;
	//m_i2DNDFImgDim = 33;
	
	m_iGonioWBand = 550;
	m_iGonioAxis = MAJORAXIS;
	
	//default is stainless IOR!!!
	m_fFresnelIORReal = 2.757;
	m_fFresnelIORImg = 3.792;

	//string filename
	ZeroMemory(m_strXYZFileName,sizeof(char)*MAX_PATH);
	//
	m_v2DGaussCenter = D3DXVECTOR2(0,0);
	//m_fSigma = 1.5f;//default value
	//m_fPixelStep = 0.25f;
	m_fSigma = 6.0f;
	m_fPixelStep = 1.0f;

	//m_fGaussianWeightLimit = 0.25f;
	m_fGaussianWeightLimit = 0.5f;
	//m_fGenMFDThreshold = 0.000001f;
	//m_fGenMFDThreshold = 0.0001f;
	m_fGenMFDThreshold = 0.1f;

	D3DXMatrixIdentity(&m_matRotNormal);
}

CXYZFileIO::~CXYZFileIO(void)
{
	reset();
}

void CXYZFileIO::reset(void)
{
	SAFE_CLOSEFILE(fp);
	SAFE_DELETE_ARRAY(m_strConstL1);
	SAFE_DELETE_ARRAY(m_strSoftwareDate);
	SAFE_DELETE_ARRAY(m_strComment);
	SAFE_DELETE_ARRAY(m_strPartSerNum);
	SAFE_DELETE_ARRAY(m_strPartNum);
	SAFE_DELETE_ARRAY(m_strObjectiveName);
	SAFE_DELETE_ARRAY(m_strSysErrFile);
	SAFE_DELETE_ARRAY(m_strZoomDesc);
	SAFE_DELETE_ARRAY(m_arrHeightMapData);
	m_arrVertices.clear();
	m_arrNormalAreaWeighted.clear();
	m_arrNoDataPtIdx.clear();
	ZeroMemory(m_strXYZFileName,sizeof(char)*MAX_PATH);
	m_arrNDFCumulation.clear();
}

bool CXYZFileIO::SplitXYZFilesArbitrarySize(unsigned int iNumRowEachPiece, unsigned int iNumColEachPiece)
{
	if(m_arrHeightMapData == NULL || m_iHMDataHeight < iNumRowEachPiece || m_iHMDataWidth < iNumColEachPiece)
		return false;
	
	int iNumPieceY1 = m_iHMDataHeight/iNumRowEachPiece;
	int iNumPieceY2 = m_iHMDataHeight%iNumRowEachPiece;
	int iNumPieceX1 = m_iHMDataWidth/iNumColEachPiece;
	int iNumPieceX2 = m_iHMDataWidth%iNumColEachPiece;
	
	int iNumPieceY = iNumPieceY1;
	if(iNumPieceY2 > 0)
		iNumPieceY += 1;

	int iNumPieceX = iNumPieceX1;
	if(iNumPieceX2 > 0)
		iNumPieceX += 1;

	//prepare file name string
	//Generate 2D normal histogram image
	CString strXYZFileFullPath(m_strXYZFileName);
	int iPos = strXYZFileFullPath.ReverseFind('\\');
	CString strXYZFileName = strXYZFileFullPath.Mid(iPos+1);
	strXYZFileFullPath.TrimRight(strXYZFileName);	
	int iPos1 = strXYZFileName.ReverseFind('.');
	CString strTmpExt = strXYZFileName.Mid(iPos1);
	strXYZFileName.TrimRight(strTmpExt);
	strXYZFileFullPath.Append(strXYZFileName);
	CreateDirectory((LPCTSTR)strXYZFileFullPath,NULL);
	strXYZFileFullPath.Append(_T("\\"));

	//save files
	bool bRt = true;
	
	for(int iFileY = 0; iFileY < iNumPieceY; iFileY++)
		for(int iFileX = 0; iFileX < iNumPieceX; iFileX++)
	{
		//prepare file Name
		CString strSplitFileFullName = strXYZFileFullPath;
		strSplitFileFullName.Append(strXYZFileName);
		CString strName;
		strName.Format(_T("_Split%03d.xyz"),iFileY*iNumPieceX + iFileX);
		strSplitFileFullName.Append(strName);
		
		unsigned int iStartRowIdx, iEndRowIdx;
		unsigned int iStartColIdx, iEndColIdx;
		//row indices
		iStartRowIdx = iFileY*iNumRowEachPiece;
		if( iNumPieceY2 > 0 && iFileY == (iNumPieceY - 1) )
			iEndRowIdx = iStartRowIdx+iNumPieceY2-1;
		else
			iEndRowIdx = iStartRowIdx+iNumRowEachPiece-1;
		//col indices
		iStartColIdx = iFileX*iNumColEachPiece;
		if (iNumPieceX2 > 0 && iFileX == (iNumPieceX - 1))
			iEndColIdx = iStartColIdx+iNumPieceX2-1;
		else
			iEndColIdx = iStartColIdx+iNumColEachPiece-1;

		bRt = SaveXYZFile(CT2A(strSplitFileFullName),iStartRowIdx,iEndRowIdx,iStartColIdx,iEndColIdx);
		if(!bRt)
			break;
		CString output;
		output.Format(_T("%s generated!"),strSplitFileFullName);
		theApp.OutputString(output);
	}
	return bRt;

}

bool CXYZFileIO::SplitXYZFilesColDir(unsigned int iNumRowEachPiece)
{
	if(m_arrHeightMapData == NULL || m_iHMDataHeight < iNumRowEachPiece)
		return false;

	int iNum1 = m_iHMDataHeight/iNumRowEachPiece;
	int iNum2 = m_iHMDataHeight%iNumRowEachPiece;

	int iNumFiles = iNum1;
	if(iNum2 > 0) 
		iNumFiles += 1;
	//prepare file name string
	//Generate 2D normal histogram image
	CString strXYZFileFullPath(m_strXYZFileName);
	int iPos = strXYZFileFullPath.ReverseFind('\\');
	CString strXYZFileName = strXYZFileFullPath.Mid(iPos+1);
	strXYZFileFullPath.TrimRight(strXYZFileName);	
	int iPos1 = strXYZFileName.ReverseFind('.');
	CString strTmpExt = strXYZFileName.Mid(iPos1);
	strXYZFileName.TrimRight(strTmpExt);
	strXYZFileFullPath.Append(strXYZFileName);
	CreateDirectory((LPCTSTR)strXYZFileFullPath,NULL);
	strXYZFileFullPath.Append(_T("\\"));

	//save files
	bool bRt = true;
	unsigned int iStartColIdx = 0;
	unsigned int iEndColIdx = m_iHMDataWidth-1;

	for(int iFile = 0; iFile < iNumFiles; iFile++)
	{
		//prepare file Name
		CString strSplitFileFullName = strXYZFileFullPath;
		strSplitFileFullName.Append(strXYZFileName);
		CString strName;
		strName.Format(_T("_Split%d.xyz"),iFile);
		strSplitFileFullName.Append(strName);

		
		unsigned int iStartRowIdx, iEndRowIdx;
		iStartRowIdx = iFile*iNumRowEachPiece;
		if( iNum2 > 0 && iFile == (iNumFiles - 1) )
			iEndRowIdx = iStartRowIdx+iNum2-1;
		else
			iEndRowIdx = iStartRowIdx+iNumRowEachPiece-1;
			
		bRt = SaveXYZFile(CT2A(strSplitFileFullName),iStartRowIdx,iEndRowIdx,iStartColIdx,iEndColIdx);
		if(!bRt)
			break;
		CString output;
		output.Format(_T("%s generated!"),strSplitFileFullName);
		theApp.OutputString(output);
	}
	return bRt;
}

bool CXYZFileIO::SplitXYZFilesRowDir(unsigned int iNumColEachPiece)
{
	if(m_arrHeightMapData == NULL || m_iHMDataWidth < iNumColEachPiece)
		return false;

	int iNum1 = m_iHMDataWidth/iNumColEachPiece;
	int iNum2 = m_iHMDataWidth%iNumColEachPiece;

	int iNumFiles = iNum1;
	if(iNum2 > 0) 
		iNumFiles += 1;
	//prepare file name string
	//Generate 2D normal histogram image
	CString strXYZFileFullPath(m_strXYZFileName);
	int iPos = strXYZFileFullPath.ReverseFind('\\');
	CString strXYZFileName = strXYZFileFullPath.Mid(iPos+1);
	strXYZFileFullPath.TrimRight(strXYZFileName);	
	int iPos1 = strXYZFileName.ReverseFind('.');
	CString strTmpExt = strXYZFileName.Mid(iPos1);
	strXYZFileName.TrimRight(strTmpExt);
	strXYZFileFullPath.Append(strXYZFileName);
	CreateDirectory((LPCTSTR)strXYZFileFullPath,NULL);
	strXYZFileFullPath.Append(_T("\\"));

	//save files
	bool bRt = true;
	unsigned int iStartRowIdx = 0;
	unsigned int iEndRowIdx = m_iHMDataHeight-1;

	for(int iFile = 0; iFile < iNumFiles; iFile++)
	{
		//prepare file Name
		CString strSplitFileFullName = strXYZFileFullPath;
		strSplitFileFullName.Append(strXYZFileName);
		CString strName;
		strName.Format(_T("_RSplit%d.xyz"),iFile);
		strSplitFileFullName.Append(strName);


		unsigned int iStartColIdx, iEndColIdx;
		iStartColIdx = iFile*iNumColEachPiece;
		if( iNum2 > 0 && iFile == (iNumFiles - 1) )
			iEndColIdx = iStartColIdx + iNum2-1;
		else
			iEndColIdx = iStartColIdx + iNumColEachPiece-1;

		bRt = SaveXYZFile(CT2A(strSplitFileFullName),iStartRowIdx,iEndRowIdx,iStartColIdx,iEndColIdx);
		if(!bRt)
			break;
		CString output;
		output.Format(_T("%s generated!"),strSplitFileFullName);
		theApp.OutputString(output);
	}
	return bRt;
}

bool CXYZFileIO::SaveOBJFile(char* strOBJFileName)
{
	if(m_arrHeightMapData == NULL || m_arrNormalAreaWeighted.size() == 0)
		return false;
	
	bool bDelFNStr = false;
	if (strOBJFileName == NULL)
	{
		//save in the same folder of .xyz file
		strOBJFileName = new char[MAX_PATH];
		ZeroMemory(strOBJFileName,sizeof(char)*MAX_PATH);
		strcpy(strOBJFileName, m_strXYZFileName);
		char* pos = strchr(strOBJFileName,'.');
		*(pos+1) = NULL;
		strcat(strOBJFileName,"obj");
		bDelFNStr = true;
	}

	//prepare vertex buffer
	//create vertex buffer
	float fStepX = m_fSampleWidth/(m_iHMDataWidth-1);
	float fStepY = m_fSampleHeight/(m_iHMDataHeight-1);
	float fStartX = -0.5f*m_fSampleWidth;
	float fStartY = -0.5f*m_fSampleHeight;
	D3DXVECTOR3* pVBData = new D3DXVECTOR3[m_iHMDataWidth*m_iHMDataHeight];
	for(unsigned int j = 0; j < m_iHMDataHeight; j++)
		for(unsigned int i = 0; i < m_iHMDataWidth; i++)
		{
			int idx = j*m_iHMDataWidth + i;
			float zval = m_arrHeightMapData[idx];
			if(zval < 0)
				zval = 0.1f;
			CD3DUtilityFunction::FillVec3(i*fStepX+fStartX, j*fStepY+fStartY, zval, pVBData+idx);			
		}
	//prepare index buffer
	unsigned int* pIBData = new unsigned int[(m_iHMDataWidth-1)*(m_iHMDataHeight-1)*6];
	for(unsigned int j = 0; j <(m_iHMDataHeight-1); j++)
		for(unsigned int i = 0; i <(m_iHMDataWidth-1); i++)
		{
			unsigned int idx = (j*(m_iHMDataWidth-1)+i)*6;
			unsigned int v1 = j*m_iHMDataWidth+i;
			unsigned int v2 = v1 + 1;
			unsigned int v3 = v1 + m_iHMDataWidth;
			unsigned int v4 = v2 + m_iHMDataWidth;
			//first triangle
			pIBData[idx++] = v1+1; pIBData[idx++] = v2+1; pIBData[idx++] = v4+1;
			//second triangle
			pIBData[idx++] = v1+1; pIBData[idx++] = v4+1; pIBData[idx++] = v3+1;

			//printf("(%d,%d,%d,%d,%d,%d)\n",v1,v3,v2,v3,v4,v2);
		}

	FILE* fp = fopen(strOBJFileName, "w");
	unsigned int iNumVertices = m_iHMDataWidth*m_iHMDataHeight;
	unsigned int iNumFaces = (m_iHMDataWidth-1)*(m_iHMDataHeight-1)*2;
	fprintf(fp,"#%d vertices, %d faces.\n", iNumVertices, iNumFaces);
	//output vertex pos
	for (unsigned int i = 0; i < iNumVertices; i++)
		fprintf(fp, "v %f %f %f\n", pVBData[i].x,pVBData[i].y,pVBData[i].z);
	////output vertex normal
	//for (unsigned int i = 0; i < m_arrNormalAreaWeighted.size(); i++)
	//	fprintf(fp, "v %f %f %f\n", m_arrNormalAreaWeighted[i].Normal.x,m_arrNormalAreaWeighted[i].Normal.y,m_arrNormalAreaWeighted[i].Normal.z);
	//output face
	for (unsigned int i = 0; i < iNumFaces; i++)
		fprintf(fp, "f %d %d %d\n",pIBData[i*3],pIBData[i*3+1],pIBData[i*3+2]);

	fclose(fp);

	//release mem
	if(bDelFNStr)
		SAFE_DELETE_ARRAY(strOBJFileName);
	SAFE_DELETE_ARRAY(pIBData);
	SAFE_DELETE_ARRAY(pVBData);

	return true;
}

bool CXYZFileIO::SaveXYZFile(char* strXYZFileName, unsigned int iStartRowIdx, unsigned int iEndRowIdx, 
	unsigned int iStartColIdx, unsigned int iEndColIdx)
{
	if(strXYZFileName == NULL || m_arrHeightMapData == NULL || iStartRowIdx >= m_iHMDataHeight || 
		iEndRowIdx >= m_iHMDataHeight || iStartRowIdx > iEndRowIdx)
		return false;
	
	//reset current object
	FILE* fp = NULL;
	if( (fp = fopen( strXYZFileName, "w" )) == NULL )
		return false;

	//start loading file
	char* tmpArray = NULL;
	NEW_CHAR_ARRAY(tmpArray, MAX_PATH);
	/*line 1*/
	//string constant 
	fprintf(fp, "%s", m_strConstL1);
	/*line 2*/
	fprintf(fp,"%d %d %d %d %s",m_iSoftwareType,m_iMajorVers,m_iMinorVers,m_iBugVers,m_strSoftwareDate);
	/*line 3*/
	int iWidth = iEndColIdx - iStartColIdx + 1;
	int iHeight = iEndRowIdx - iStartRowIdx + 1;
	fprintf(fp,"%d %d %d %d %d %u\n",m_iIntensOriginX, m_iIntensOriginY, iWidth, iHeight, m_iNBuckets, m_uiIntensRange);
	/*line 4*/
	fprintf(fp,"%d %d %d %d\n",m_iPhaseOriginX,m_iPhaseOriginY,iWidth,iHeight);
	/*line 5*/
	fprintf(fp,"%s",m_strComment);
	/*line 6*/
	fprintf(fp,"%s",m_strPartSerNum);
	/*line 7*/
	fprintf(fp,"%s",m_strPartNum);
	/*line 8*/
	fprintf(fp,"%d %f %.1e %f %f %f %e %d\n",m_iSource, m_fIntfScaleFactor, m_fWavelengthIn, m_fNumericAperture, m_fObliquityFactor, m_fMagnification, m_fCameraRes, m_iTimeStamp);
	/*line 9*/
	fprintf(fp,"%d %d %d %d %d %d %s", iWidth,iHeight,m_iSystemType,m_iSystemBoard,m_iSystemSerial,m_iInstrumentId,m_strObjectiveName);
	/*line 10*/
	fprintf(fp,"%d %d %d %d %d %d %f %f %d %d\n",m_iAcquireMode,m_iIntensAvgs,m_iPZTCal,m_iPZTGain,m_iPZTGainTolerance,m_iAGC,m_fTargetRange,m_fLightLevel,m_iMinMod,m_iMinModPts);
	/*line 11*/
	fprintf(fp,"%d %d %d %d %d %d %d %d %d\n",m_iPhaseRes,m_iPhaseAvgs,m_iMinimumAreaSize,m_iDisconAction,m_fDisconFilter,m_iConnectionOrder,m_iRemoveTiltBias,m_iDataSign,m_iCodeVType);
	/*line 12*/
	fprintf(fp,"%d %s", m_iSubtractSysErr,m_strSysErrFile);
	/*line 13*/
	fprintf(fp,"%f %f\n",m_fRefractiveIndex, m_fPartThickness);
	/*line 14*/
	fprintf(fp,"%s",m_strZoomDesc);

	//start to write data!!!
	char* strNoData = "No Data";
	for (unsigned int j = iStartRowIdx; j <= iEndRowIdx; j++)
		for (unsigned int i = iStartColIdx; i<=iEndColIdx; i++)
		{
			float fHeight = *(m_arrHeightMapData+j*m_iHMDataWidth+i);
			if(fHeight < 0)
				fprintf(fp, "%d %d %s\n", i, j, strNoData);
			else
				fprintf(fp, "%d %d %f\n", i, j, fHeight);
		}
		fprintf(fp,"#");
	fclose(fp);

	return true;
}

void CXYZFileIO::OutputDebugInfoThread(UtilityString& strDbgInfo)
{
	CString* pOutputStr = new CString(strDbgInfo.GetCharStr());
	theApp.PostThreadMessage(WM_OUTPUT_DEBUGMSGTHREAD, WPARAM(pOutputStr),0);
}

int CXYZFileIO::LoadFromFile(const char* strFileName,float fSampleWidth, float fSampleHeight)
{
	if(strFileName == NULL)
		return -1;

	//reset current object
	reset();
	if( (fp = fopen( strFileName, "r" )) == NULL )
		 return -1;
	
	//start loading file
	UtilityString strDebug;
	strDebug.GenerateFormatSmallString("start loading %s file\n",strFileName);
	OutputDebugInfoThread(strDebug);

	char* tmpArray = NULL;
	NEW_CHAR_ARRAY(tmpArray, MAX_PATH);
	/*line 1*/
	strDebug.GenerateFormatSmallString("File header:\n");
	OutputDebugInfoThread(strDebug);
	//string constant 
	NEW_CHAR_ARRAY(m_strConstL1,MAX_PATH);
	fgets(tmpArray,MAX_PATH,fp);
	strcpy(m_strConstL1,tmpArray);
	
	strDebug.SetCharStr(m_strConstL1);
	OutputDebugInfoThread(strDebug);
	/*line 2*/
	NEW_CHAR_ARRAY(m_strSoftwareDate,MAX_PATH);
	ZeroMemory(tmpArray, MAX_PATH);
	fgets(tmpArray, MAX_PATH, fp);
	char* tmpStrStart = strchr(tmpArray, '"');
	strcpy(m_strSoftwareDate, tmpStrStart);
	*tmpStrStart = '\0';
	sscanf(tmpArray,"%d %d %d %d",&m_iSoftwareType,&m_iMajorVers,&m_iMinorVers,&m_iBugVers);
	printf("%d %d %d %d %s",m_iSoftwareType,m_iMajorVers,m_iMinorVers,m_iBugVers,m_strSoftwareDate);
	/*line 3*/
	fscanf(fp,"%d %d %d %d %d %u\n",&m_iIntensOriginX, &m_iIntensOriginY, &m_iIntensWidth, &m_iIntensHeight, &m_iNBuckets, &m_uiIntensRange);
	printf("%d %d %d %d %d %u\n",m_iIntensOriginX, m_iIntensOriginY, m_iIntensWidth, m_iIntensHeight, m_iNBuckets, m_uiIntensRange);
	/*line 4*/
	fscanf(fp,"%d %d %d %d\n",&m_iPhaseOriginX,&m_iPhaseOriginY,&m_iPhaseWidth,&m_iPhaseHeight);
	printf("%d %d %d %d\n",m_iPhaseOriginX,m_iPhaseOriginY,m_iPhaseWidth,m_iPhaseHeight);
	/*line 5*/
	NEW_CHAR_ARRAY(m_strComment,MAX_PATH);
	ZeroMemory(tmpArray,MAX_PATH);
	fgets(tmpArray, MAX_PATH, fp);
	strcpy(m_strComment,tmpArray);
	printf("%s",m_strComment);
	/*line 6*/
	NEW_CHAR_ARRAY(m_strPartSerNum, MAX_PATH);
	ZeroMemory(tmpArray, MAX_PATH);
	fgets(tmpArray, MAX_PATH, fp);
	strcpy(m_strPartSerNum, tmpArray);
	printf("%s",m_strPartSerNum);
	/*line 7*/
	NEW_CHAR_ARRAY(m_strPartNum, MAX_PATH);
	ZeroMemory(tmpArray, MAX_PATH);
	fgets(tmpArray, MAX_PATH, fp);
	strcpy(m_strPartNum, tmpArray);
	printf("%s",m_strPartNum);
	/*line 8*/
	fscanf(fp,"%d %f %f %f %f %f %f %d\n",&m_iSource, &m_fIntfScaleFactor, &m_fWavelengthIn, &m_fNumericAperture, &m_fObliquityFactor, &m_fMagnification, &m_fCameraRes, &m_iTimeStamp);
	printf("%d %f %f %f %f %f %f %d\n",m_iSource, m_fIntfScaleFactor, m_fWavelengthIn, m_fNumericAperture, m_fObliquityFactor, m_fMagnification, m_fCameraRes, m_iTimeStamp);
	/*line 9*/
	NEW_CHAR_ARRAY(m_strObjectiveName, MAX_PATH);
	ZeroMemory(tmpArray, MAX_PATH);
	fgets(tmpArray, MAX_PATH, fp);
	tmpStrStart = strchr(tmpArray,'"');
	strcpy(m_strObjectiveName, tmpStrStart);
	*tmpStrStart = '\0';
	sscanf(tmpArray,"%d %d %d %d %d %d",&m_iCameraWidth,&m_iCameraHeight,&m_iSystemType,&m_iSystemBoard,&m_iSystemSerial,&m_iInstrumentId);
	printf("%d %d %d %d %d %d %s",m_iCameraWidth,m_iCameraHeight,m_iSystemType,m_iSystemBoard,m_iSystemSerial,m_iInstrumentId,m_strObjectiveName);
	/*line 10*/
	fscanf(fp,"%d %d %d %d %d %d %f %f %d %d\n",&m_iAcquireMode,&m_iIntensAvgs,&m_iPZTCal,&m_iPZTGain,&m_iPZTGainTolerance,&m_iAGC,&m_fTargetRange,&m_fLightLevel,&m_iMinMod,&m_iMinModPts);
	printf("%d %d %d %d %d %d %f %f %d %d\n",m_iAcquireMode,m_iIntensAvgs,m_iPZTCal,m_iPZTGain,m_iPZTGainTolerance,m_iAGC,m_fTargetRange,m_fLightLevel,m_iMinMod,m_iMinModPts);
	/*line 11*/
	fscanf(fp,"%d %d %d %d %d %d %d %d %d\n",&m_iPhaseRes,&m_iPhaseAvgs,&m_iMinimumAreaSize,&m_iDisconAction,&m_fDisconFilter,&m_iConnectionOrder,&m_iRemoveTiltBias,&m_iDataSign,&m_iCodeVType);
	printf("%d %d %d %d %d %d %d %d %d\n",m_iPhaseRes,m_iPhaseAvgs,m_iMinimumAreaSize,m_iDisconAction,m_fDisconFilter,m_iConnectionOrder,m_iRemoveTiltBias,m_iDataSign,m_iCodeVType);
	/*line 12*/
	NEW_CHAR_ARRAY(m_strSysErrFile, MAX_PATH);
	ZeroMemory(tmpArray, MAX_PATH);
	fgets(tmpArray, MAX_PATH, fp);
	tmpStrStart = strchr(tmpArray,'"');
	strcpy(m_strSysErrFile, tmpStrStart);
	*tmpStrStart = '\0';
	sscanf(tmpArray, "%d", &m_iSubtractSysErr);
	printf("%d %s", m_iSubtractSysErr,m_strSysErrFile);
	/*line 13*/
	fscanf(fp, "%f %f\n",&m_fRefractiveIndex, &m_fPartThickness);
	printf("%f %f\n",m_fRefractiveIndex, m_fPartThickness);
	/*line 14*/
	NEW_CHAR_ARRAY(m_strZoomDesc, MAX_PATH);
	ZeroMemory(tmpArray, MAX_PATH);
	fgets(tmpArray, MAX_PATH, fp);
	strcpy(m_strZoomDesc,tmpArray);
	printf("%s",m_strZoomDesc);
	printf("End of header.\n");
	
	/***************************************************************************************/
	//height map data
	if(m_iPhaseWidth>0&&m_iPhaseHeight>0)
	{
		m_iHMDataWidth = m_iPhaseWidth;
		m_iHMDataHeight = m_iPhaseHeight;
		
		int iTotalPixels = m_iHMDataWidth*m_iHMDataHeight;
		int iBatchNum = 1000;
				
		printf("Loading data part...\n");
		SAFE_DELETE_ARRAY(m_arrHeightMapData);
		m_arrHeightMapData = new float[iTotalPixels];
		int iXpos = 0, iYpos = 0;
		float fMinHeight = -MINVAL;
		float fMaxHeight = MINVAL;
		ZeroMemory(tmpArray, MAX_PATH);
		m_arrNoDataPtIdx.clear();
		
		for(unsigned int j = 0; j < m_iHMDataHeight; j++)
			for(unsigned int i = 0; i < m_iHMDataWidth; i++)
			{
				int iIdx = j*m_iHMDataWidth+i;
				//showing progress!
				if(iIdx%iBatchNum == 0)
				{
					strDebug.GenerateFormatSmallString("%d%% processed", (int)(float(iIdx) * 100 / iTotalPixels));
					OutputDebugInfoThread(strDebug);
					//PostMessage(theMainWnd, WM_LOADXYZ_PROCESSING,
						//WPARAM(float(iIdx) * 100 / iTotalPixels) ,	// percentage
						//0// 
						//);
				}

				float* pHeight = (m_arrHeightMapData+iIdx);
				
				fgets(tmpArray,MAX_PATH,fp);
				char* tmp = strchr(tmpArray,'N');
				if(tmp==NULL)
					{
						sscanf(tmpArray, "%d %d %f", &iYpos, &iXpos, pHeight);
						if(*pHeight < fMinHeight)
							fMinHeight = *pHeight;
						if(*pHeight > fMaxHeight)
							fMaxHeight = *pHeight;
				}
				else
					{
						*pHeight = MINVAL;
						m_arrNoDataPtIdx.push_back(UnsolvedPoint(i,j));						
					}
				//printf("%d %d %f\n", iYpos, iXpos, *(m_arrHeightMapData+j*m_iPhaseWidth+i));
			}
			//convert the datarange to [0,1]
			m_fHeightRange = fMaxHeight - fMinHeight;
			for(unsigned int j = 0; j < m_iHMDataHeight; j++)
				for(unsigned int i = 0; i < m_iHMDataWidth; i++)
				{
					float* pHeight = (m_arrHeightMapData+j*m_iHMDataWidth+i);
					if(!(*pHeight < MINVAL + 1.0))
						*pHeight = (*pHeight - fMinHeight);///(fMaxHeight - fMinHeight);						
					else
						//for "No data" point, height = -0.01f
						//*pHeight = -0.01f;
						*pHeight = -0.1f;
				}

		m_bDataReady = true;
		strDebug.GenerateFormatSmallString("Loading data finished.\n");
		OutputDebugInfoThread(strDebug);
		strDebug.GenerateFormatSmallString("Number of \'No data\' points is %d, ratio:%f%%\n",m_arrNoDataPtIdx.size(), GetNoDataRatio()*100);
		OutputDebugInfoThread(strDebug);
		strDebug.GenerateFormatSmallString("Height range is Min:%f, Max:%f, Diff:%f.\n", fMinHeight, fMaxHeight, m_fHeightRange);
		OutputDebugInfoThread(strDebug);
	}
	else
	{
		UtilityString strDebug1("Error: No data in File...");
		OutputDebugInfoThread(strDebug1);
	}
	
	SAFE_DELETE_ARRAY(tmpArray);
	SAFE_CLOSEFILE(fp);

	m_fSampleWidth = fSampleWidth;
	m_fSampleHeight = fSampleHeight;
	//set file name
	SetStrXYZFilePathName(strFileName);

	//save height bmp file
	UtilityString strHeightBMPFile(m_strXYZFileName);
	strHeightBMPFile.replaceExt("bmp");
	bool bRt = saveHeightmap2D(strHeightBMPFile.GetCharStr());
	if(bRt)
	{
		UtilityString strDebug2;
		strDebug2.GenerateFormatSmallString("%s Generated!", strHeightBMPFile.GetCharStr());
		OutputDebugInfoThread(strDebug2);
	}

	return 0;
}

bool CXYZFileIO::saveHeightmap2D(const char* strFileName)
{
	if (m_arrHeightMapData == NULL || m_iHMDataWidth <=0 || m_iHMDataHeight <=0)
		return false;

	//output buffer
	CImg<unsigned char> output(m_iHMDataWidth,m_iHMDataHeight,1,3,0);

	for (unsigned int j = 0; j < m_iHMDataHeight; j++)
		for (unsigned int i = 0; i < m_iHMDataWidth; i++)
		{
			int iIdx = j*m_iHMDataWidth + i;
			float fHeight = m_arrHeightMapData[iIdx];
			
			if(fHeight >=0){
				fHeight /= m_fHeightRange;//normalization
				output.atXY(i,j,0,0) = (unsigned char)(255*fHeight);			
				output.atXY(i,j,0,1) = (unsigned char)(255*fHeight);			
				output.atXY(i,j,0,2) = (unsigned char)(255*fHeight);			
			} else { //no-data point
				output.atXY(i,j,0,0) = 255;			
				output.atXY(i,j,0,1) = 255;			
				output.atXY(i,j,0,2) = 0;			
			}
		}
	//save file!
	output.save_bmp(strFileName);
	return true;
}

bool CXYZFileIO::SetNewNormalWndSize(int iSize)
{
	if(m_iCurNormalEstiMethod == USEAREAWEIGHTEDNORMAL)
		return false;

	if(iSize == m_iNormalWndSize)
		return true;
	//odd always
	if(iSize%2==0)
		iSize++;
	
	unsigned int iOldSize = m_iNormalWndSize;
	m_iNormalWndSize = iSize;
	bool bSuccess = EstimateNormalForEachPoint();
	if(!bSuccess)
		m_iNormalWndSize = iOldSize;
	return bSuccess;	
}

bool CXYZFileIO::SetGaussianSigma(float fSigma)
{
	if(m_iCurNormalEstiMethod != USEWEIGHTEDPCA_GAUSSIAN)
		return false;
	if(fabs(m_fSigma-fSigma)< 0.00001)//equal
		return true;
	if(fSigma <= 0 )
		return false;

	float fOldSigma = m_fSigma;
	m_fSigma = fSigma;
	bool bSuccess = EstimateNormalForEachPoint();
	if(!bSuccess)
		m_fSigma = fOldSigma;
	return bSuccess;
}

bool CXYZFileIO::SetNormalEstiMethod(NORMALESTIMATEMETHOD iCurMethod) 
{
	if(iCurMethod == m_iCurNormalEstiMethod)
		return true;

	NORMALESTIMATEMETHOD iOldMethod = m_iCurNormalEstiMethod;
	m_iCurNormalEstiMethod = iCurMethod;
	bool bRt = EstimateNormalForEachPoint();
	if(!bRt)
		m_iCurNormalEstiMethod = iOldMethod;
	return bRt;
}

void CXYZFileIO::ComputeNDFCumulationArray_Largeregion(CXYZFileIO* pSmallXYZFile)
{
	if(!pSmallXYZFile || pSmallXYZFile->m_arrNormalAreaWeighted.size() == 0)
		return;
	int iDim = pSmallXYZFile->GetCur2DNDFImgDim();
	if(m_arrNDFCumulation_LargeRegion.size() == 0)
	{
		for (int j = 0; j < iDim; j++)
			for (int i = 0; i < iDim; i++)
				m_arrNDFCumulation_LargeRegion.push_back(0);	
	}
	//loop all normals
	for (int i = 0; i < pSmallXYZFile->m_arrNormalAreaWeighted.size(); i++)
	{
		//D3DXVECTOR3 norm = pSmallXYZFile->m_arrNormalAreaWeighted[i].Normal;
		D3DXVECTOR3 norm;;
		D3DXVec3TransformNormal(&norm,&(pSmallXYZFile->m_arrNormalAreaWeighted[i].Normal),&m_matRotNormal);

		norm.x = norm.x/2.0f + 0.5f; //(-1,1) --> (0,1); 
		norm.y = norm.y/2.0f + 0.5f;
		int iPosX = (int)(floorf(norm.x * iDim));
		int iPosY = (int)(floorf(norm.y * iDim));
		if(iPosX == iDim) iPosX--;
		if(iPosY == iDim) iPosY--;
		unsigned int idx = iPosY*iDim + iPosX;
		m_arrNDFCumulation_LargeRegion[idx] += 1.0;
	}	
}

bool CXYZFileIO::EstimateNormalForEachPoint_MultipleFiles(CString& strFNs, NORMALESTIMATEMETHOD iCurNFMethod, 
	int iKernelSize, float fGaussSigma, float fSampleWidth, float fSampleHeight )
{

	CString strSelectedFiles(strFNs);
	strSelectedFiles.TrimLeft();
	strSelectedFiles.TrimRight();

	int iPos = strSelectedFiles.ReverseFind(_T('\\'));
	CString strFileNames = strSelectedFiles.Mid(iPos+1);
	CString strFileFolderPath = strSelectedFiles.TrimRight(strFileNames);
	strFileNames.TrimLeft();strFileNames.TrimRight();

	bool bFinished = false;
	//Get filtering method, kernel size, sigma from GUI
	//
	int iNFKernelSize = iKernelSize;
	float fCurGaussSigma = fGaussSigma;
	float fCurSampleWidth = fSampleWidth;
	float fCurSampleHeight = fSampleHeight;

	UtilityString strDebug("Start Generate NDF from multiple files:");
	OutputDebugInfoThread(strDebug);

	CString strLogFN = strFileFolderPath;
	strLogFN.Append(L"LargeRegion_2DNDF.log");
	
	strDebug.SetWCharStr(strLogFN.GetBuffer());
	OutputDebugInfoThread(strDebug);
	
	FILE* fp = NULL;
	_tfopen_s(&fp,(const TCHAR*)(strLogFN.GetBuffer()), L"w");
	_ftprintf(fp, L"iCurNFMethod=%d, iNFKernelSize=%d, fCurGaussSigma=%f\n", iCurNFMethod, iNFKernelSize, fCurGaussSigma);
	//clear array
	m_arrNDFCumulation_LargeRegion.clear();
	int i2DNDFImgDim = m_i2DNDFImgDim;
	for (int j = 0; j < i2DNDFImgDim; j++)
		for (int i = 0; i < i2DNDFImgDim; i++)
			m_arrNDFCumulation_LargeRegion.push_back(0.0);	

	srand ( time(NULL) );

	while (!bFinished)
	{
		CString strSingleFN;
		int iPos1 = strFileNames.Find(_T(' '));
		if(iPos1 >= 0)
			strSingleFN = strFileNames.Left(iPos1);
		else
		{
			strSingleFN = strFileNames;
			bFinished = true;
		}
		strFileNames.TrimLeft(strSingleFN);
		strFileNames.TrimLeft();
		CString strSingleFileFullPath = strFileFolderPath + strSingleFN;

		//theApp.OutputString(strSingleFileFullPath);
		//process each file!
		CXYZFileIO* pSingleXYZFile = new CXYZFileIO();
		//load files
		if(pSingleXYZFile->LoadFromFile(CT2A(strSingleFileFullPath),fSampleWidth,fSampleHeight) < 0)
			return false;
		//temporarily change the members 
		pSingleXYZFile->m_iCurNormalEstiMethod = iCurNFMethod;
		pSingleXYZFile->m_fSigma = fCurGaussSigma;
		pSingleXYZFile->m_iNormalWndSize = iNFKernelSize;

		//estimate normals
		if(!pSingleXYZFile->EstimateNormalForEachPointNoAccumulation())
			return false;
#if 0
		//randonmize the rotation matrix
		float fAng = 0.5f*((float)rand())/RAND_MAX;//radian value in (0,0.5)
		D3DXMatrixRotationX(&m_matRotNormal,fAng);
		strDebug.GenerateFormatSmallString("Rotation Angle: %f degrees", 180*fAng/D3DX_PI);
		OutputDebugInfoThread(strDebug);
#endif
		//accumulation Normal
		ComputeNDFCumulationArray_Largeregion(pSingleXYZFile);

		//delete
		SAFE_DELETE(pSingleXYZFile);
		UtilityString strTmp(strSingleFileFullPath.GetBuffer());
		strDebug.GenerateFormatSmallString("%s processed!", strTmp.GetCharStr());
		OutputDebugInfoThread(strDebug);

		//Save log file
		_ftprintf(fp,L"%s\n",strSingleFileFullPath.GetBuffer());
	}
	//close log
	fclose(fp);
	return true;
}

bool CXYZFileIO::EstimateNormalForEachPointNoAccumulation()
{
	bool bRt = true;

	switch(m_iCurNormalEstiMethod)
	{
	case USEWEIGHTEDPCA_GAUSSIAN:
		bRt = EstimateNormalForEachPoint_WeightedPCA_IgnoreNoDataPt();
		break;
	case USEPCANORMAL:
		bRt = EstimateNormalForEachPoint_PCA_IgnoreNoDataPt();
		break;
	case USEAREAWEIGHTEDNORMAL:
		bRt = EstimateNormalAreaWeighted_IgnoreNoDataPt();
		break;
	}

	return bRt;
}

bool CXYZFileIO::EstimateNormalForEachPoint()
{
	bool bRt = true;

	switch(m_iCurNormalEstiMethod)
	{
	case USEWEIGHTEDPCA_GAUSSIAN:
		bRt = EstimateNormalForEachPoint_WeightedPCA_IgnoreNoDataPt();
		break;
	case USEPCANORMAL:
		//bRt = EstimateNormalForEachPoint_PCA_IgnoreNoDataPt();
		bRt = EstimateNormalForEachPoint_PCAZonly_IgnoreNoDataPt_Eigen();
		break;
	case USEAREAWEIGHTEDNORMAL:
		bRt = EstimateNormalAreaWeighted_IgnoreNoDataPt();
		break;
	}

	//save 2D NDF image
	ComputeNDFCumulationArray();
	HRESULT hr = Save2DNDFImg();
	if(hr!=S_OK)
			bRt = false;

	return bRt;
}

bool CXYZFileIO::EstimateNormalForEachPoint_Eigen()
{
	bool bRt = true;

	switch(m_iCurNormalEstiMethod)
	{
	case USEWEIGHTEDPCA_GAUSSIAN:
		bRt = EstimateNormalForEachPoint_WeightedPCA_IgnoreNoDataPt_Eigen();
		break;
	case USEPCANORMAL:
		bRt = EstimateNormalForEachPoint_PCA_IgnoreNoDataPt_Eigen();
		break;
	case USEAREAWEIGHTEDNORMAL:
		bRt = EstimateNormalAreaWeighted_IgnoreNoDataPt();
		break;
	}

	//save 2D NDF image
	ComputeNDFCumulationArray();
	HRESULT hr = Save2DNDFImg();
	if(hr!=S_OK)
		bRt = false;

	return bRt;
}


/*
Finding the orthogonal distance regression plane is an eigenvector 
problem. It is quite involved, as you suspected. The best solution 
utilizes the Singular Value Decomposition (SVD). I hope you have 
access to a good linear algebra software package.

Starting with the distance from a point to a plane, we wish to find 
a, b, c and d such that we minimize

f(a,b,c,d) = Sum [|axi + byi + czi + d|^2 / (a^2 + b^2 + c^2)]

If we set the partial derivative with respect to d equal to zero, we 
can solve for d to get

d = -(a*x0 + b*y0 + c*z0)

where (x0, y0, z0) is the centroid of the data. This means that the 
least squares plane contains the centroid. If we substitute it back 
into the equation for the plane we get

a(x - x0) + b(y - y0) + c(z - z0) = 0

We can rewrite f(a,b,c,d) like this

f(a,b,c) = Sum [|a(xi-x0) + b(yi-y0) + c(zi-z0)|^2 / (a^2+b^2+c^2)]

Now we are going to switch over to a matrix representation. I will 
be using both the upper and lower cases of some letters. I hope that 
does not cause confusion.

Let's define v and M such that

 T
v  = [a  b  c]

--                             --
| x1 - x0    y1 - y0    z1 - z0 |
| x2 - x0    y2 - y0    z2 - z0 |
M =  |    .          .          .    |
|    .          .          .    |
|    .          .          .    |
| xn - x0    yn - y0    zn - z0 |
--                             --

If you multiply the matrices out, you will see that f(a,b,c) becomes

         T T          T
f(v) = (v M )(Mv) / (v v)

   T  T        T
= v (M M)v / (v v)

                    T
Let's define A = M M, which when divided by the number of data points 
becomes the covariance matrix of the data.

f(v) is called a Rayleigh Quotient. It is minimized by the eigenvector 
of A that corresponds to its smallest eigenvalue.

We could compute the eigenvectors of A, but this is not needed. The 
SVD of M is
       T
M = USV 

where S is a diagonal matrix containing the singular values of M, the 
columns of V are its singular vectors, and U is an orthogonal matrix.

                    T
Now            A = M M

      T T    T
= (USV ) (USV )

      T T      T
= (V S U ) (USV )

   2 T 
= V S V

This decomposition of A diagonalizes the matrix and provides an 
eigenvector decomposition. It means that the eigenvalues of A are the 
squares of the singular values of M, and the eigenvectors of A are the 
singular vectors of M.

To conclude, the orthogonal least squares 3D plane contains the 
centroid of the data, and its normal vector is the singular vector of 
M corresponding to its smallest singular value.
*/
bool CXYZFileIO::GenerateVerticesPos(float fStartOffsetX, float fStartOffsetY)
{
	if(!m_bDataReady)
		return false;

	//create vertex buffer
	//float fStepX = m_fSampleWidth/(m_iHMDataWidth-1);
	//float fStepY = m_fSampleHeight/(m_iHMDataHeight-1);
	float fStepX = m_fSampleWidth/(m_iHMDataWidth);
	float fStepY = m_fSampleHeight/(m_iHMDataHeight);
	float fStartX = fStartOffsetX*m_fSampleWidth;
	float fStartY = fStartOffsetY*m_fSampleHeight;
	for(unsigned int j = 0; j < m_iHMDataHeight; j++)
		for(unsigned int i = 0; i < m_iHMDataWidth; i++)
		{	
			VertexNorm tmpPos;
			int idx = j*m_iHMDataWidth + i;
			//CD3DUtilityFunction::FillVec3(i*fStepX+fStartX, j*fStepY+fStartY, m_arrHeightMapData[idx], &(tmpPos.Position));			
			CD3DUtilityFunction::FillVec3((i+0.5f)*fStepX+fStartX, (j+0.5f)*fStepY+fStartY, m_arrHeightMapData[idx], &(tmpPos.Position));			
			m_arrVertices.push_back(tmpPos);
		}
	return true;
}

/*
You might want to check out principal component analysis. You can calculate a 3x3 covariance matrix for 
your set of points and then determine the eigenvectors corresponding to the two largest eigenvalues. 
These two vectors are parallel to the best-fit plane, so just cross them to get the plane's normal, 
and then adjust the plane's fourth coordinate so that it passed through the average position of your point set.

Here I am using 3x3 covariance matrix to save the computation for svdcmp();

Note! the array element index value for Numerical Recipes functions starts from 1 not 0!
*/
bool CXYZFileIO::EstimateNormalForEachPoint_PCA()
{
	bool bRtVal = true;

	if(!m_bDataReady)
		return false;
	//Firstly fill the vertices array using positions
	if(m_arrVertices.size() != m_iHMDataWidth*m_iHMDataHeight)
	{
		m_arrVertices.clear();
		bRtVal = GenerateVerticesPos(-0.5f,-0.5f);
		if (!bRtVal)
			return bRtVal;
	}

	//start to estimate normal
	std::vector<UnsolvedPoint> NoDataPts_Arr;
	UtilityString strDebug("Start to compute normal for each point!\n");
	OutputDebugInfoThread(strDebug);

	unsigned int iNeg = 0;
	//Loop each point
	for (unsigned int iY = 0; iY < m_iHMDataHeight; iY++)
		for (unsigned int iX = 0; iX < m_iHMDataWidth; iX++)
		{   
			unsigned int iCurPtIdx = iY*m_iHMDataWidth + iX;
			if(iCurPtIdx%(m_iHMDataWidth*10)==0)
			{
				strDebug.GenerateFormatSmallString("*");
				OutputDebugInfoThread(strDebug);
			}
			//'No-data' point case
			if(m_arrHeightMapData[iCurPtIdx] < 0)
			{
				NoDataPts_Arr.push_back(UnsolvedPoint(iX,iY));
				continue;
			}
			//1.compute the average position
			int iStep = m_iNormalWndSize/2;
			D3DXVECTOR3 iCenterPos(0,0,0);
			std::vector<D3DXVECTOR3> LocalPts_Arr;
			for (int j = -iStep; j <= iStep; j++)
				for(int i = -iStep; i <= iStep; i++)
				{
					int idx_x = i+iX;
					int idx_y = j+iY;
					if(idx_x < 0 || idx_y < 0 || idx_x >= (int)(m_iHMDataWidth) || idx_y >= (int)(m_iHMDataHeight))
						continue;

					unsigned int idx = idx_y*m_iHMDataWidth+idx_x;
					//'No-data' point case
					if(m_arrHeightMapData[idx] < 0)
						continue;

					iCenterPos += m_arrVertices[idx].Position;					
					LocalPts_Arr.push_back(m_arrVertices[idx].Position);
				}
				//there is no enough valid points in neighborhood!
				size_t iNumLocalPts = LocalPts_Arr.size();
				if (iNumLocalPts < 3)
				{
					NoDataPts_Arr.push_back(UnsolvedPoint(iX,iY));
					continue;
				}

				//Average position
				iCenterPos /= (float)(iNumLocalPts);

				//2.Construct convariance matrix M*M_T
				double** CovMat = new double*[4];
				double **V = new double*[4];
				double W[4];
				for(int i = 0; i < 4; i++)
				{
					CovMat[i] = new double[4];
					V[i] = new double[4];
				}
				
				std::vector<double> xvec;
				std::vector<double> yvec;
				std::vector<double> zvec;
				for(unsigned int i = 0; i < iNumLocalPts; i++)
				{
					xvec.push_back(LocalPts_Arr[i].x - iCenterPos.x);
					yvec.push_back(LocalPts_Arr[i].y - iCenterPos.y);
					zvec.push_back(LocalPts_Arr[i].z - iCenterPos.z);
				}

				//compute elements
				CovMat[1][1] = 0;
				for(unsigned int i =0; i < iNumLocalPts; i++)
					CovMat[1][1] += xvec[i] * xvec[i];

				CovMat[1][2] = 0;
				for(unsigned int i =0; i < iNumLocalPts; i++)
					CovMat[1][2] += xvec[i] * yvec[i];

				CovMat[1][3] = 0;
				for(unsigned int i =0; i < iNumLocalPts; i++)
					CovMat[1][3] += xvec[i] * zvec[i];

				CovMat[2][1] = CovMat[1][2];
				CovMat[2][2] = 0;
				for(unsigned int i =0; i < iNumLocalPts; i++)
					CovMat[2][2] += yvec[i] * yvec[i];

				CovMat[2][3] = 0;
				for(unsigned int i =0; i < iNumLocalPts; i++)
					CovMat[2][3] += yvec[i] * zvec[i];

				CovMat[3][1] = CovMat[1][3];
				CovMat[3][2] = CovMat[2][3];
				CovMat[3][3] = 0;
				for(unsigned int i =0; i < iNumLocalPts; i++)
					CovMat[3][3] += zvec[i] * zvec[i];

				if (!SVD::svdcmp(CovMat,3,3,W,V))
				{
#ifdef _DEBUG
					OUTPUTERRORINFO;
#endif
					return false;
				}

				int iMinIdx = -1;
				int iMaxIdx = -1;
				double minVal = DBL_MAX;
				double maxVal = DBL_MIN;
				for(int k = 1; k <4; k++)
				{
					if(W[k] < minVal)
					{
						minVal = W[k];
						iMinIdx = k;
					}

					if(W[k] > maxVal)
					{
						maxVal = W[k];
						iMaxIdx = k;
					}
				}
				
				D3DXVECTOR3 norm = D3DXVECTOR3(V[1][iMinIdx],V[2][iMinIdx],V[3][iMinIdx]);
				if(norm.z < 0)
				{
					//printf("(%f,%f,%f)\t", norm.x,norm.y,norm.z);
					norm *= -1.0f;
					//iNeg++;
				}
				//3. Fill the normal of current pt
				D3DXVec3Normalize(&(m_arrVertices[iCurPtIdx].Normal),&norm);
											
				//release memory
				for (int i = 0; i < 4; i++)
				{
					SAFE_DELETE_ARRAY(CovMat[i]);
					SAFE_DELETE_ARRAY(V[i]);
				}
				SAFE_DELETE_ARRAY(CovMat);
				SAFE_DELETE_ARRAY(V);
		}
		strDebug.GenerateFormatSmallString("\niNeg:%d\n",iNeg);
		OutputDebugInfoThread(strDebug);

		//process normal of 'No-data' Point
		//use local triangle's normal vector
		for (unsigned int i = 0; i < NoDataPts_Arr.size();i++)
		{
			unsigned int idY = NoDataPts_Arr[i].iPtY;
			unsigned int idX = NoDataPts_Arr[i].iPtX;

			unsigned int iCurPtIdx = idY*m_iHMDataWidth + idX;
			unsigned int v2 = iCurPtIdx + 1;
			unsigned int v3 = iCurPtIdx + m_iHMDataWidth;
			unsigned int v4 = v2 + m_iHMDataWidth;
			
			int iCase = -1;
			//normal case (1)(v2-v1)x(v4-v1) (2)(v4-v1)x(v3-v1)
			if(idX < (m_iHMDataWidth-1) && idY < (m_iHMDataHeight-1))
			{
				D3DXVECTOR3 vec1 = m_arrVertices[v2].Position - m_arrVertices[iCurPtIdx].Position;//v2-v1
				D3DXVECTOR3 vec2 = m_arrVertices[v4].Position - m_arrVertices[iCurPtIdx].Position;//v4-v1

				D3DXVECTOR3 norm;
				D3DXVec3Cross(&norm,&vec1,&vec2);
				D3DXVECTOR3 norm1;
				D3DXVec3Normalize(&norm1,&norm);

				vec1 = m_arrVertices[v3].Position - m_arrVertices[iCurPtIdx].Position;//v3-v1
				D3DXVec3Cross(&norm,&vec2,&vec1);
				D3DXVECTOR3 norm2;
				D3DXVec3Normalize(&norm2,&norm);
				//simple average, and we can go to complex area-based way!!!
				norm = (norm1+norm2)/2.0f;
				D3DXVec3Normalize(&(m_arrVertices[iCurPtIdx].Normal),&norm);				
				iCase = 1;
				if(m_arrVertices[iCurPtIdx].Normal.z < 0)
				{
					strDebug.GenerateFormatSmallString("%d:(%f,%f,%f)\t", iCase, m_arrVertices[iCurPtIdx].Normal.x,m_arrVertices[iCurPtIdx].Normal.y,m_arrVertices[iCurPtIdx].Normal.z);
					OutputDebugInfoThread(strDebug);
					iNeg++;
				}			
			}
			//(2)(v3-v1)x(v4-v1) (2)(v4-v1)x(v2-v1)
			else if (idY < (m_iHMDataHeight-1))
			{
				v2 = iCurPtIdx - 1;
				v3 = iCurPtIdx + m_iHMDataWidth;
				v4 = v2 + m_iHMDataWidth;

				D3DXVECTOR3 vec1 = m_arrVertices[v3].Position - m_arrVertices[iCurPtIdx].Position;//v3-v1
				D3DXVECTOR3 vec2 = m_arrVertices[v4].Position - m_arrVertices[iCurPtIdx].Position;//v4-v1

				D3DXVECTOR3 norm;
				D3DXVec3Cross(&norm,&vec1,&vec2);
				D3DXVECTOR3 norm1;
				D3DXVec3Normalize(&norm1,&norm);

				vec1 = m_arrVertices[v2].Position - m_arrVertices[iCurPtIdx].Position;//v2-v1
				D3DXVec3Cross(&norm,&vec2,&vec1);
				D3DXVECTOR3 norm2;
				D3DXVec3Normalize(&norm2,&norm);

				norm = (norm1+norm2)/2.0f;
				D3DXVec3Normalize(&(m_arrVertices[iCurPtIdx].Normal),&norm);	
				iCase = 2;
				if(m_arrVertices[iCurPtIdx].Normal.z < 0)
				{
					strDebug.GenerateFormatSmallString("%d:(%f,%f,%f)\t", iCase, m_arrVertices[iCurPtIdx].Normal.x,m_arrVertices[iCurPtIdx].Normal.y,m_arrVertices[iCurPtIdx].Normal.z);
					OutputDebugInfoThread(strDebug);
					iNeg++;
				}			
			}
			//(3)(v3-v1)x(v4-v1) (2)(v4-v1)x(v2-v1)
			else if (idX < (m_iHMDataWidth-1))
			{
				unsigned int v2 = iCurPtIdx + 1;
				unsigned int v3 = iCurPtIdx - m_iHMDataWidth;
				unsigned int v4 = v2 - m_iHMDataWidth;
				
				D3DXVECTOR3 vec1 = m_arrVertices[v3].Position - m_arrVertices[iCurPtIdx].Position;//v3-v1
				D3DXVECTOR3 vec2 = m_arrVertices[v4].Position - m_arrVertices[iCurPtIdx].Position;//v4-v1

				D3DXVECTOR3 norm;
				D3DXVec3Cross(&norm,&vec1,&vec2);
				D3DXVECTOR3 norm1;
				D3DXVec3Normalize(&norm1,&norm);

				vec1 = m_arrVertices[v2].Position - m_arrVertices[iCurPtIdx].Position;//v2-v1
				D3DXVec3Cross(&norm,&vec2,&vec1);
				D3DXVECTOR3 norm2;
				D3DXVec3Normalize(&norm2,&norm);

				norm = (norm1+norm2)/2.0f;
				D3DXVec3Normalize(&(m_arrVertices[iCurPtIdx].Normal),&norm);
				iCase = 3;
				if(m_arrVertices[iCurPtIdx].Normal.z < 0)
				{
					strDebug.GenerateFormatSmallString("%d:(%f,%f,%f)\t", iCase, m_arrVertices[iCurPtIdx].Normal.x,m_arrVertices[iCurPtIdx].Normal.y,m_arrVertices[iCurPtIdx].Normal.z);
					OutputDebugInfoThread(strDebug);
					iNeg++;
				}			
			}
			//(4)(v2-v1)x(v4-v1) (2)(v4-v1)x(v3-v1)
			else if (idX == (m_iHMDataWidth-1) && idY == (m_iHMDataHeight-1))
			{
				unsigned int v2 = iCurPtIdx - 1;
				unsigned int v3 = iCurPtIdx - m_iHMDataWidth;
				unsigned int v4 = v2 - m_iHMDataWidth;

				D3DXVECTOR3 vec1 = m_arrVertices[v2].Position - m_arrVertices[iCurPtIdx].Position;//v2-v1
				D3DXVECTOR3 vec2 = m_arrVertices[v4].Position - m_arrVertices[iCurPtIdx].Position;//v4-v1

				D3DXVECTOR3 norm;
				D3DXVec3Cross(&norm,&vec1,&vec2);
				D3DXVECTOR3 norm1;
				D3DXVec3Normalize(&norm1,&norm);

				vec1 = m_arrVertices[v3].Position - m_arrVertices[iCurPtIdx].Position;//v3-v1
				D3DXVec3Cross(&norm,&vec2,&vec1);
				D3DXVECTOR3 norm2;
				D3DXVec3Normalize(&norm2,&norm);

				norm = (norm1+norm2)/2.0f;
				D3DXVec3Normalize(&(m_arrVertices[iCurPtIdx].Normal),&norm);	
				iCase = 4;
				if(m_arrVertices[iCurPtIdx].Normal.z < 0)
				{
					strDebug.GenerateFormatSmallString("%d:(%f,%f,%f)\t", iCase, m_arrVertices[iCurPtIdx].Normal.x,m_arrVertices[iCurPtIdx].Normal.y,m_arrVertices[iCurPtIdx].Normal.z);
					OutputDebugInfoThread(strDebug);
					iNeg++;
				}			
			}

		}	

		strDebug.GenerateFormatSmallString("\niNeg:%d\n",iNeg);
		OutputDebugInfoThread(strDebug);
		strDebug.GenerateFormatSmallString("\n Normal computing is finished!\n");
		OutputDebugInfoThread(strDebug);

	return true;
}


bool CXYZFileIO::EstimateNormalForEachPoint_PCA_IgnoreNoDataPt_Eigen()
{
	bool bRtVal = true;

	CHiPerfTimer timer;
	timer.Start();

	if(!m_bDataReady)
		return false;
	//Firstly fill the vertices array using positions
	if(m_arrVertices.size() != m_iHMDataWidth*m_iHMDataHeight)
	{
		m_arrVertices.clear();
		bRtVal = GenerateVerticesPos(-0.5f,-0.5f);
		if(!bRtVal)
			return bRtVal;
	}

	//start to estimate normal
	std::vector<UnsolvedPoint> NoDataPts_Arr;
	UtilityString strDebug("Start to compute normal for each point!\n");
	OutputDebugInfoThread(strDebug);
	//Loop each point
	m_arrNormalAreaWeighted.clear();

	for (unsigned int iY = 0; iY < m_iHMDataHeight; iY++)
		for (unsigned int iX = 0; iX < m_iHMDataWidth; iX++)
		{   
			unsigned int iCurPtIdx = iY*m_iHMDataWidth + iX;
			if(iCurPtIdx%(m_iHMDataWidth*10)==0)
			{
				strDebug.GenerateFormatSmallString("*");
				OutputDebugInfoThread(strDebug);
			}
			//ignore 'No-data' point case
			if(m_arrHeightMapData[iCurPtIdx] < 0)
				continue;
			//1.compute the average position
			int iStep = m_iNormalWndSize/2;
			std::vector<D3DXVECTOR3> LocalPts_Arr;
			for (int j = -iStep; j <= iStep; j++)
				for(int i = -iStep; i <= iStep; i++)
				{
					int idx_x = i+iX;
					int idx_y = j+iY;
					if(idx_x < 0 || idx_y < 0 || idx_x >= (int)(m_iHMDataWidth) || idx_y >= (int)(m_iHMDataHeight))
						continue;

					unsigned int idx = idx_y*m_iHMDataWidth+idx_x;
					//'No-data' point case
					if(m_arrHeightMapData[idx] < 0)
						continue;
					LocalPts_Arr.push_back(m_arrVertices[idx].Position);
				}
				//there is no enough valid points in neighborhood!
				size_t iNumLocalPts = LocalPts_Arr.size();
				if (iNumLocalPts < 3)
				{
					NoDataPts_Arr.push_back(UnsolvedPoint(iX,iY));
					continue;
				}

				//2.Construct convariance matrix M*M_T
				MatrixXd LocalPtsPos(iNumLocalPts,3);
				for (unsigned int iPt = 0; iPt < iNumLocalPts; iPt++)
				{
					LocalPtsPos(iPt,0) = LocalPts_Arr[iPt].x;
					LocalPtsPos(iPt,1) = LocalPts_Arr[iPt].y;
					LocalPtsPos(iPt,2) = LocalPts_Arr[iPt].z;
				}
				MatrixXd centered = LocalPtsPos.rowwise() - LocalPtsPos.colwise().mean();
				Matrix3d CovMat = centered.transpose() * centered;
				EigenSolver<Matrix3d> eig(CovMat);

				double eig0 = eig.eigenvalues()(0).real();
				double eig1 = eig.eigenvalues()(1).real();
				double eig2 = eig.eigenvalues()(2).real();

				int idx = 1;
				if(eig0<=eig1 && eig0<=eig2)
					idx = 0;
				if(eig2<=eig0 && eig2<=eig1)
					idx = 2;

				D3DXVECTOR3 norm = D3DXVECTOR3(eig.eigenvectors().col(idx)(0).real(),eig.eigenvectors().col(idx)(1).real(),eig.eigenvectors().col(idx)(2).real());

				if(norm.z < 0)
					norm *= -1.0f;

				//3. Fill the normal of current pt
				VertexNorm vert;
				vert.Position = m_arrVertices[iCurPtIdx].Position;
				D3DXVec3Normalize(&(vert.Normal),&norm);
				m_arrNormalAreaWeighted.push_back(vert);				
		}

		strDebug.GenerateFormatSmallString("NumUnsolvedPt:%d\n",NoDataPts_Arr.size());
		OutputDebugInfoThread(strDebug);
		strDebug.GenerateFormatSmallString("\n Normal computing is finished!\n");
		OutputDebugInfoThread(strDebug);

		timer.Stop();
		strDebug.GenerateFormatSmallString("timing: %f ms\n", timer.GetDuration());
		OutputDebugInfoThread(strDebug);

		return true;
}

bool CXYZFileIO::EstimateNormalForEachPoint_PCAZonly_IgnoreNoDataPt_Eigen()
{
	bool bRtVal = true;

	CHiPerfTimer timer;
	timer.Start();

	if(!m_bDataReady)
		return false;
	//Firstly fill the vertices array using positions
	if(m_arrVertices.size() != m_iHMDataWidth*m_iHMDataHeight)
	{
		m_arrVertices.clear();
		bRtVal = GenerateVerticesPos(-0.5f,-0.5f);
		if(!bRtVal)
			return bRtVal;
	}

	//start to estimate normal
	std::vector<UnsolvedPoint> NoDataPts_Arr;
	UtilityString strDebug("Start to compute normal for each point!\n");
	OutputDebugInfoThread(strDebug);
	//Loop each point
	m_arrNormalAreaWeighted.clear();

	for (unsigned int iY = 0; iY < m_iHMDataHeight; iY++)
		for (unsigned int iX = 0; iX < m_iHMDataWidth; iX++)
		{   
			unsigned int iCurPtIdx = iY*m_iHMDataWidth + iX;
			if(iCurPtIdx%(m_iHMDataWidth*10)==0)
			{
				strDebug.GenerateFormatSmallString("*");
				OutputDebugInfoThread(strDebug);
			}
			//ignore 'No-data' point case
			if(m_arrHeightMapData[iCurPtIdx] < 0)
				continue;
			//1.compute the average position
			int iStep = m_iNormalWndSize/2;
			std::vector<D3DXVECTOR3> LocalPts_Arr;
			for (int j = -iStep; j <= iStep; j++)
				for(int i = -iStep; i <= iStep; i++)
				{
					int idx_x = i+iX;
					int idx_y = j+iY;
					if(idx_x < 0 || idx_y < 0 || idx_x >= (int)(m_iHMDataWidth) || idx_y >= (int)(m_iHMDataHeight))
						continue;

					unsigned int idx = idx_y*m_iHMDataWidth+idx_x;
					//'No-data' point case
					if(m_arrHeightMapData[idx] < 0)
						continue;
					LocalPts_Arr.push_back(m_arrVertices[idx].Position);
				}
				//there is no enough valid points in neighborhood!
				size_t iNumLocalPts = LocalPts_Arr.size();
				if (iNumLocalPts < 5)
				{
					NoDataPts_Arr.push_back(UnsolvedPoint(iX,iY));
					continue;
				}

				//2.Construct matrix A and B
				Matrix3d matA(3,3);
				matA.setZero();
				Vector3d matB;
				matB.setZero();
				for (unsigned int iPt = 0; iPt < iNumLocalPts; iPt++) {
					double xPos = LocalPts_Arr[iPt].x;
					double yPos = LocalPts_Arr[iPt].y;
					double zPos = LocalPts_Arr[iPt].z;

					matA(0,0) = matA(0,0) + xPos*xPos;
					matA(0,1) = matA(0,1) + xPos*yPos;
					matA(0,2) = matA(0,2) + xPos;
					matA(1,0) = matA(1,0) + xPos*yPos;
					matA(1,1) = matA(1,1) + yPos*yPos;
					matA(1,2) = matA(1,2) + yPos;
					matA(2,0) = matA(2,0) + xPos;
					matA(2,1) = matA(2,1) + yPos;
					matA(2,2) = matA(2,2) + 1;
					
					matB(0) = matB(0) + xPos*zPos;
					matB(1) = matB(1) + yPos*zPos;
					matB(2) = matB(2) + zPos;
				}
				
				//solve coefficents
				Vector3d matCoeffs;
				matCoeffs = matA.inverse() * matB;
				double coeff_A = matCoeffs(0);
				double coeff_B = matCoeffs(1);
				double coeff_C = matCoeffs(2);
				D3DXVECTOR3 vec(-coeff_A, -coeff_B, 1.0f);
				D3DXVECTOR3 norm;
				D3DXVec3Normalize(&norm, &vec);
				if(norm.z < 0)
					norm *= -1.0f;

				//3. Fill the normal of current pt
				VertexNorm vert;
				vert.Position = m_arrVertices[iCurPtIdx].Position;
				vert.Normal = norm;
				m_arrNormalAreaWeighted.push_back(vert);				
		}

		strDebug.GenerateFormatSmallString("NumUnsolvedPt:%d\n",NoDataPts_Arr.size());
		OutputDebugInfoThread(strDebug);
		strDebug.GenerateFormatSmallString("\n Normal computing is finished!\n");
		OutputDebugInfoThread(strDebug);

		timer.Stop();
		strDebug.GenerateFormatSmallString("timing: %f ms\n", timer.GetDuration());
		OutputDebugInfoThread(strDebug);

		return true;
}

//#define WRITEOUTINDEX
//#define WRITEOUTKERNELREGION
bool CXYZFileIO::EstimateNormalForEachPoint_PCA_IgnoreNoDataPt()
{
	CHiPerfTimer timer;
	timer.Start();

	bool bRtVal = true;

	if(!m_bDataReady)
		return false;
	//Firstly fill the vertices array using positions
	if(m_arrVertices.size() != m_iHMDataWidth*m_iHMDataHeight)
	{
		m_arrVertices.clear();
		bRtVal = GenerateVerticesPos(-0.5f,-0.5f);
		if(!bRtVal)
			return bRtVal;
	}

	//start to estimate normal
	std::vector<UnsolvedPoint> NoDataPts_Arr;
	UtilityString strDebug("Start to compute normal for each point!\n");
	OutputDebugInfoThread(strDebug);
	//Loop each point
	m_arrNormalAreaWeighted.clear();
	//debug
	int iNumWeirdNorm = 0;

#ifdef WRITEOUTINDEX
	FILE* fp = NULL;
	fopen_s(&fp, "debugWeirdNorm.txt", "w");
#endif

#ifdef WRITEOUTKERNELREGION
	std::map<unsigned int, UnsolvedPoint> arrWeirdNormPts;

	FILE* fp = NULL;
	fopen_s(&fp, "debugWeirdNorm.txt", "r");
	typedef pair<unsigned int, UnsolvedPoint> coordPair;

	while (!feof(fp))
	{
		unsigned int iX1, iY1;
		float fAng;
		fscanf_s(fp,"%d %d %f\n",&iX1,&iY1,&fAng);
		//compute hash value
		unsigned int iZHashVal = CD3DUtilityFunction::coordToZOrderHash_2D(iX1,iY1);
		strDebug.GenerateFormatSmallString("iZHashVal=%d\n",iZHashVal);
		OutputDebugInfoThread(strDebug);
		arrWeirdNormPts.insert(coordPair(iZHashVal, UnsolvedPoint(iX1,iY1)));
	}
	fclose(fp);
#endif
	for (unsigned int iY = 0; iY < m_iHMDataHeight; iY++)
		for (unsigned int iX = 0; iX < m_iHMDataWidth; iX++)
		{   
			unsigned int iCurPtIdx = iY*m_iHMDataWidth + iX;
			if(iCurPtIdx%(m_iHMDataWidth*10)==0)
			{
				strDebug.GenerateFormatSmallString("*");
				OutputDebugInfoThread(strDebug);
			}
			//ignore 'No-data' point case
			if(m_arrHeightMapData[iCurPtIdx] < 0)
					continue;
			//1.compute the average position
			int iStep = m_iNormalWndSize/2;
			D3DXVECTOR3 iCenterPos(0,0,0);
			std::vector<D3DXVECTOR3> LocalPts_Arr;
			for (int j = -iStep; j <= iStep; j++)
				for(int i = -iStep; i <= iStep; i++)
				{
					int idx_x = i+iX;
					int idx_y = j+iY;
					if(idx_x < 0 || idx_y < 0 || idx_x >= (int)(m_iHMDataWidth) || idx_y >= (int)(m_iHMDataHeight))
						continue;

					unsigned int idx = idx_y*m_iHMDataWidth+idx_x;
					//'No-data' point case
					if(m_arrHeightMapData[idx] < 0)
						continue;

					iCenterPos += m_arrVertices[idx].Position;					
					LocalPts_Arr.push_back(m_arrVertices[idx].Position);
				}
				//there is no enough valid points in neighborhood!
				size_t iNumLocalPts = LocalPts_Arr.size();
				if (iNumLocalPts < 3)
				{
					NoDataPts_Arr.push_back(UnsolvedPoint(iX,iY));
					continue;
				}

#ifdef WRITEOUTKERNELREGION
				map <unsigned int, UnsolvedPoint> :: const_iterator iterElem;
				unsigned int iZHashVal = CD3DUtilityFunction::coordToZOrderHash_2D(iX,iY);
				iterElem = arrWeirdNormPts.find(iZHashVal);
				//if((iX==611) && (iY==23)){
				if(iterElem!=arrWeirdNormPts.end()){
					UtilityString FN;
					FN.GenerateFormatSmallString("LNeighbor_iX=%diY=%d.txt", iX, iY);

					FILE* fp = NULL;
					fopen_s(&fp, FN.GetCharStr(), "w");

					for (size_t iIdx = 0; iIdx < LocalPts_Arr.size(); iIdx++)
					{
						fprintf(fp, "%f %f %f\n", LocalPts_Arr[iIdx].x,LocalPts_Arr[iIdx].y,LocalPts_Arr[iIdx].z);
					}
					fclose(fp);
				}
#endif

				//Average position
				iCenterPos /= (float)(iNumLocalPts);

				//2.Construct convariance matrix M*M_T
				double** CovMat = new double*[4];
				double **V = new double*[4];
				double W[4];
				for(int i = 0; i < 4; i++)
				{
					CovMat[i] = new double[4];
					V[i] = new double[4];
				}

				std::vector<double> xvec;
				std::vector<double> yvec;
				std::vector<double> zvec;
				for(unsigned int i = 0; i < iNumLocalPts; i++)
				{
					xvec.push_back(LocalPts_Arr[i].x - iCenterPos.x);
					yvec.push_back(LocalPts_Arr[i].y - iCenterPos.y);
					zvec.push_back(LocalPts_Arr[i].z - iCenterPos.z);
				}

				//compute elements
				CovMat[1][1] = 0;
				for(unsigned int i =0; i < iNumLocalPts; i++)
					CovMat[1][1] += xvec[i] * xvec[i];

				CovMat[1][2] = 0;
				for(unsigned int i =0; i < iNumLocalPts; i++)
					CovMat[1][2] += xvec[i] * yvec[i];

				CovMat[1][3] = 0;
				for(unsigned int i =0; i < iNumLocalPts; i++)
					CovMat[1][3] += xvec[i] * zvec[i];

				CovMat[2][1] = CovMat[1][2];
				CovMat[2][2] = 0;
				for(unsigned int i =0; i < iNumLocalPts; i++)
					CovMat[2][2] += yvec[i] * yvec[i];

				CovMat[2][3] = 0;
				for(unsigned int i =0; i < iNumLocalPts; i++)
					CovMat[2][3] += yvec[i] * zvec[i];

				CovMat[3][1] = CovMat[1][3];
				CovMat[3][2] = CovMat[2][3];
				CovMat[3][3] = 0;
				for(unsigned int i =0; i < iNumLocalPts; i++)
					CovMat[3][3] += zvec[i] * zvec[i];

				if (!SVD::svdcmp(CovMat,3,3,W,V))
				{
					return false;
				}

				int iMinIdx = -1;
				//int iMaxIdx = -1;
				double minVal = DBL_MAX;
				//double maxVal = DBL_MIN;
				for(int k = 1; k <4; k++)
				{
					if(W[k] < minVal)
					{
						minVal = W[k];
						iMinIdx = k;
					}
					//if(W[k] > maxVal)
					//{
					//	maxVal = W[k];
					//	iMaxIdx = k;
					//}
				}

				D3DXVECTOR3 norm = D3DXVECTOR3(V[1][iMinIdx],V[2][iMinIdx],V[3][iMinIdx]);
				if(norm.z < 0)
					norm *= -1.0f;
					
				//3. Fill the normal of current pt
				VertexNorm vert;
				vert.Position = m_arrVertices[iCurPtIdx].Position;
				D3DXVec3Normalize(&(vert.Normal),&norm);
				m_arrNormalAreaWeighted.push_back(vert);
#ifdef WRITEOUTINDEX
				D3DXVECTOR3 mNorm(0,0,1);
				norm = vert.Normal;
				float ldotn = D3DXVec3Dot(&norm,&mNorm);
				float theta = acos(ldotn)*RADIAN2DEGREE;
				if((90-theta) < 2)	{
					fprintf(fp,"%d %d %f\n", iX,iY,theta);
					iNumWeirdNorm++;
				}
#endif
				//release memory
				for (int i = 0; i < 4; i++)
				{
					SAFE_DELETE_ARRAY(CovMat[i]);
					SAFE_DELETE_ARRAY(V[i]);
				}
				SAFE_DELETE_ARRAY(CovMat);
				SAFE_DELETE_ARRAY(V);
		}
		
		strDebug.GenerateFormatSmallString("NumUnsolvedPt:%d\n",NoDataPts_Arr.size());
		OutputDebugInfoThread(strDebug);
		//process normal of 'No-data' Point
		//use local triangle's normal vector
		//unsigned int iNeg = 0;
		//for (unsigned int i = 0; i < NoDataPts_Arr.size();i++)
		//{
		//	unsigned int idY = NoDataPts_Arr[i].iPtY;
		//	unsigned int idX = NoDataPts_Arr[i].iPtX;

		//	unsigned int iCurPtIdx = idY*m_iHMDataWidth + idX;
		//	unsigned int v2 = iCurPtIdx + 1;
		//	unsigned int v3 = iCurPtIdx + m_iHMDataWidth;
		//	unsigned int v4 = v2 + m_iHMDataWidth;

		//	int iCase = -1;
		//	//normal case (1)(v2-v1)x(v4-v1) (2)(v4-v1)x(v3-v1)
		//	if(idX < (m_iHMDataWidth-1) && idY < (m_iHMDataHeight-1))
		//	{
		//		D3DXVECTOR3 vec1 = m_arrVertices[v2].Position - m_arrVertices[iCurPtIdx].Position;//v2-v1
		//		D3DXVECTOR3 vec2 = m_arrVertices[v4].Position - m_arrVertices[iCurPtIdx].Position;//v4-v1

		//		D3DXVECTOR3 norm;
		//		D3DXVec3Cross(&norm,&vec1,&vec2);
		//		D3DXVECTOR3 norm1;
		//		D3DXVec3Normalize(&norm1,&norm);

		//		vec1 = m_arrVertices[v3].Position - m_arrVertices[iCurPtIdx].Position;//v3-v1
		//		D3DXVec3Cross(&norm,&vec2,&vec1);
		//		D3DXVECTOR3 norm2;
		//		D3DXVec3Normalize(&norm2,&norm);
		//		//simple average, and we can go to complex area-based way!!!
		//		norm = (norm1+norm2)/2.0f;
		//		VertexNorm vert;
		//		vert.Position = m_arrVertices[iCurPtIdx].Position;
		//		D3DXVec3Normalize(&(vert.Normal),&norm);
		//		m_arrNormalAreaWeighted.push_back(vert);

		//		iCase = 1;
		//		if(vert.Normal.z < 0)
		//		{
		//			strDebug.GenerateFormatSmallString("%d:(%f,%f,%f)\t", iCase, vert.Normal.x,vert.Normal.y,vert.Normal.z);
		//			OutputDebugInfoThread(strDebug);
		//			iNeg++;
		//		}			
		//	}
		//	//(2)(v3-v1)x(v4-v1) (2)(v4-v1)x(v2-v1)
		//	else if (idY < (m_iHMDataHeight-1))
		//	{
		//		v2 = iCurPtIdx - 1;
		//		v3 = iCurPtIdx + m_iHMDataWidth;
		//		v4 = v2 + m_iHMDataWidth;

		//		D3DXVECTOR3 vec1 = m_arrVertices[v3].Position - m_arrVertices[iCurPtIdx].Position;//v3-v1
		//		D3DXVECTOR3 vec2 = m_arrVertices[v4].Position - m_arrVertices[iCurPtIdx].Position;//v4-v1

		//		D3DXVECTOR3 norm;
		//		D3DXVec3Cross(&norm,&vec1,&vec2);
		//		D3DXVECTOR3 norm1;
		//		D3DXVec3Normalize(&norm1,&norm);

		//		vec1 = m_arrVertices[v2].Position - m_arrVertices[iCurPtIdx].Position;//v2-v1
		//		D3DXVec3Cross(&norm,&vec2,&vec1);
		//		D3DXVECTOR3 norm2;
		//		D3DXVec3Normalize(&norm2,&norm);

		//		norm = (norm1+norm2)/2.0f;
		//		VertexNorm vert;
		//		vert.Position = m_arrVertices[iCurPtIdx].Position;
		//		D3DXVec3Normalize(&(vert.Normal),&norm);
		//		m_arrNormalAreaWeighted.push_back(vert);

		//		iCase = 2;
		//		if(vert.Normal.z < 0)
		//		{
		//			strDebug.GenerateFormatSmallString("%d:(%f,%f,%f)\t", iCase, vert.Normal.x,vert.Normal.y,vert.Normal.z);
		//			OutputDebugInfoThread(strDebug);
		//			iNeg++;
		//		}			
		//	}
		//	//(3)(v3-v1)x(v4-v1) (2)(v4-v1)x(v2-v1)
		//	else if (idX < (m_iHMDataWidth-1))
		//	{
		//		unsigned int v2 = iCurPtIdx + 1;
		//		unsigned int v3 = iCurPtIdx - m_iHMDataWidth;
		//		unsigned int v4 = v2 - m_iHMDataWidth;

		//		D3DXVECTOR3 vec1 = m_arrVertices[v3].Position - m_arrVertices[iCurPtIdx].Position;//v3-v1
		//		D3DXVECTOR3 vec2 = m_arrVertices[v4].Position - m_arrVertices[iCurPtIdx].Position;//v4-v1

		//		D3DXVECTOR3 norm;
		//		D3DXVec3Cross(&norm,&vec1,&vec2);
		//		D3DXVECTOR3 norm1;
		//		D3DXVec3Normalize(&norm1,&norm);

		//		vec1 = m_arrVertices[v2].Position - m_arrVertices[iCurPtIdx].Position;//v2-v1
		//		D3DXVec3Cross(&norm,&vec2,&vec1);
		//		D3DXVECTOR3 norm2;
		//		D3DXVec3Normalize(&norm2,&norm);

		//		norm = (norm1+norm2)/2.0f;
		//		VertexNorm vert;
		//		vert.Position = m_arrVertices[iCurPtIdx].Position;
		//		D3DXVec3Normalize(&(vert.Normal),&norm);
		//		m_arrNormalAreaWeighted.push_back(vert);

		//		iCase = 3;
		//		if(vert.Normal.z < 0)
		//		{
		//			strDebug.GenerateFormatSmallString("%d:(%f,%f,%f)\t", iCase, vert.Normal.x,vert.Normal.y,vert.Normal.z);
		//			OutputDebugInfoThread(strDebug);
		//			iNeg++;
		//		}			
		//	}
		//	//(4)(v2-v1)x(v4-v1) (2)(v4-v1)x(v3-v1)
		//	else if (idX == (m_iHMDataWidth-1) && idY == (m_iHMDataHeight-1))
		//	{
		//		unsigned int v2 = iCurPtIdx - 1;
		//		unsigned int v3 = iCurPtIdx - m_iHMDataWidth;
		//		unsigned int v4 = v2 - m_iHMDataWidth;

		//		D3DXVECTOR3 vec1 = m_arrVertices[v2].Position - m_arrVertices[iCurPtIdx].Position;//v2-v1
		//		D3DXVECTOR3 vec2 = m_arrVertices[v4].Position - m_arrVertices[iCurPtIdx].Position;//v4-v1

		//		D3DXVECTOR3 norm;
		//		D3DXVec3Cross(&norm,&vec1,&vec2);
		//		D3DXVECTOR3 norm1;
		//		D3DXVec3Normalize(&norm1,&norm);

		//		vec1 = m_arrVertices[v3].Position - m_arrVertices[iCurPtIdx].Position;//v3-v1
		//		D3DXVec3Cross(&norm,&vec2,&vec1);
		//		D3DXVECTOR3 norm2;
		//		D3DXVec3Normalize(&norm2,&norm);

		//		norm = (norm1+norm2)/2.0f;
		//		VertexNorm vert;
		//		vert.Position = m_arrVertices[iCurPtIdx].Position;
		//		D3DXVec3Normalize(&(vert.Normal),&norm);
		//		m_arrNormalAreaWeighted.push_back(vert);

		//		iCase = 4;
		//		if(vert.Normal.z < 0)
		//		{
		//			strDebug.GenerateFormatSmallString("%d:(%f,%f,%f)\t", iCase, vert.Normal.x,vert.Normal.y,vert.Normal.z);
		//			OutputDebugInfoThread(strDebug);
		//			iNeg++;
		//		}			
		//	}

		//}	

		//strDebug.GenerateFormatSmallString("\niNeg:%d\n",iNeg);
		//OutputDebugInfoThread(strDebug);
		strDebug.GenerateFormatSmallString("\n Normal computing is finished!\n");
		OutputDebugInfoThread(strDebug);

		timer.Stop();
		strDebug.GenerateFormatSmallString("timing: %f ms\n", timer.GetDuration());
		OutputDebugInfoThread(strDebug);

#ifdef WRITEOUTINDEX
		fclose(fp);
		strDebug.GenerateFormatSmallString("iNumWeirdNorm=%d",iNumWeirdNorm);
		OutputDebugInfoThread(strDebug);
#endif
		return true;
}

bool CXYZFileIO::EstimateNormalForEachPoint_WeightedPCA_IgnoreNoDataPt_Eigen()
{
	bool bRtVal = true;

	CHiPerfTimer timer;
	timer.Start();

	if(!m_bDataReady)
		return false;
	//Firstly fill the vertices array using positions
	if(m_arrVertices.size() != m_iHMDataWidth*m_iHMDataHeight)
	{
		m_arrVertices.clear();
		bRtVal = GenerateVerticesPos(-0.5f,-0.5f);
		if(!bRtVal)
			return bRtVal;
	}

	//start to estimate normal
	std::vector<UnsolvedPoint> NoDataPts_Arr;
	UtilityString strDebug("Start to compute normal for each point!\n");
	OutputDebugInfoThread(strDebug);

	m_arrNormalAreaWeighted.clear();
	//prepare the Gaussian weight array
	std::vector<double> arr_GaussianWeights;
	m_iNormalWndSize = CD3DUtilityFunction::Compute2DUniformGaussianKernel(m_fSigma,m_fPixelStep,arr_GaussianWeights);
	//Loop each point
	for ( unsigned int iY = 0; iY < m_iHMDataHeight; iY++)
		for ( unsigned int iX = 0; iX < m_iHMDataWidth; iX++)
		{   
			unsigned int iCurPtIdx = iY*m_iHMDataWidth + iX;
			if(iCurPtIdx%(m_iHMDataWidth*10)==0)
			{
				strDebug.GenerateFormatSmallString("*");
				OutputDebugInfoThread(strDebug);
			}
			//ignore 'No-data' point case
			if(m_arrHeightMapData[iCurPtIdx] < 0)
			{
				NoDataPts_Arr.push_back(UnsolvedPoint(iX,iY));
				continue;
			}
			//1.compute the average position
			int iStep = m_iNormalWndSize/2;
			//the w channel for storing Gaussian weight
			std::vector<D3DXVECTOR4> LocalPts_Arr;
			float fTotalGaussianWeight = 0.0f;
			for (int j = -iStep; j <= iStep; j++)
				for(int i = -iStep; i <= iStep; i++)
				{
					int idx_x = i+iX;
					int idx_y = j+iY;
					if(idx_x < 0 || idx_y < 0 || idx_x >= (int)(m_iHMDataWidth) || idx_y >= (int)(m_iHMDataHeight))
						continue;

					unsigned int idx = idx_y*m_iHMDataWidth+idx_x;
					//'No-data' point case
					if(m_arrHeightMapData[idx] < 0)
						continue;

					unsigned int iGaussWeightIdx = (j+iStep)*m_iNormalWndSize + (i+iStep);
					float fGaussianWeight = arr_GaussianWeights[iGaussWeightIdx];
					LocalPts_Arr.push_back(D3DXVECTOR4(m_arrVertices[idx].Position,fGaussianWeight));
					fTotalGaussianWeight += fGaussianWeight;
				}
				//there is no enough valid points in neighborhood!
				size_t iNumLocalPts = LocalPts_Arr.size();
				if ((fTotalGaussianWeight < m_fGaussianWeightLimit) || (iNumLocalPts < 3) )
				{
					NoDataPts_Arr.push_back(UnsolvedPoint(iX,iY));
					continue;
				}

				//1.weighted average center
				//D3DXVECTOR3 vCenterPos(0,0,0);
				MatrixXd LocalPtsPos(iNumLocalPts,3);
				VectorXd vGaussianWeights(iNumLocalPts);

				for(unsigned int idx = 0; idx < iNumLocalPts; idx++)
				{
					LocalPtsPos(idx,0) = LocalPts_Arr[idx].x;
					LocalPtsPos(idx,1) = LocalPts_Arr[idx].y;
					LocalPtsPos(idx,2) = LocalPts_Arr[idx].z;
					////Renormalize weight values
					LocalPts_Arr[idx].w /= fTotalGaussianWeight;
					//vCenterPos += D3DXVECTOR3(LocalPts_Arr[idx].x,LocalPts_Arr[idx].y,LocalPts_Arr[idx].z)*LocalPts_Arr[idx].w;	
					vGaussianWeights(idx) = LocalPts_Arr[idx].w;
				}

				//2.Construct convariance matrix M*M_T
				MatrixXd centered = (LocalPtsPos.rowwise() - vGaussianWeights.transpose() * LocalPtsPos);
				MatrixXd matGaussianWeights(iNumLocalPts,3);
				matGaussianWeights.col(0) = vGaussianWeights;
				matGaussianWeights.col(1) = vGaussianWeights;
				matGaussianWeights.col(2) = vGaussianWeights;
				MatrixXd centeredGaussian = centered.cwiseProduct(matGaussianWeights);
				Matrix3d CovMat = centeredGaussian.transpose() * centeredGaussian;
				EigenSolver<Matrix3d> eig(CovMat);


				double eig0 = eig.eigenvalues()(0).real();
				double eig1 = eig.eigenvalues()(1).real();
				double eig2 = eig.eigenvalues()(2).real();

				int idx = 1;
				if(eig0<=eig1 && eig0<=eig2)
					idx = 0;
				if(eig2<=eig0 && eig2<=eig1)
					idx = 2;

				D3DXVECTOR3 norm = D3DXVECTOR3(eig.eigenvectors().col(idx)(0).real(),eig.eigenvectors().col(idx)(1).real(),eig.eigenvectors().col(idx)(2).real());

				if(norm.z < 0)
					norm *= -1.0f;

				//3. Fill the normal of current pt
				VertexNorm vert;
				vert.Position = m_arrVertices[iCurPtIdx].Position;
				D3DXVec3Normalize(&(vert.Normal),&norm);
				m_arrNormalAreaWeighted.push_back(vert);
				
		}

		strDebug.GenerateFormatSmallString("\n NumsolvedPt:%d, NumUnsolvedPt:%d\n",m_arrNormalAreaWeighted.size(),NoDataPts_Arr.size());
		OutputDebugInfoThread(strDebug);
		strDebug.GenerateFormatSmallString("\n Normal computing is finished!\n");
		OutputDebugInfoThread(strDebug);

		timer.Stop();
		strDebug.GenerateFormatSmallString("\n timing: %f ms\n", timer.GetDuration());
		OutputDebugInfoThread(strDebug);

		return true;
}

bool CXYZFileIO::EstimateNormalForEachPoint_WeightedPCA_IgnoreNoDataPt()
{
	bool bRtVal = true;

	CHiPerfTimer timer;
	timer.Start();

	if(!m_bDataReady)
		return false;
	//Firstly fill the vertices array using positions
	if(m_arrVertices.size() != m_iHMDataWidth*m_iHMDataHeight)
	{
		m_arrVertices.clear();
		bRtVal = GenerateVerticesPos(-0.5f,-0.5f);
		if(!bRtVal)
			return bRtVal;
	}

	//start to estimate normal
	std::vector<UnsolvedPoint> NoDataPts_Arr;
	UtilityString strDebug("Start to compute normal for each point!\n");
	OutputDebugInfoThread(strDebug);

	m_arrNormalAreaWeighted.clear();
	//prepare the Gaussian weight array
	std::vector<double> arr_GaussianWeights;
	m_iNormalWndSize = CD3DUtilityFunction::Compute2DUniformGaussianKernel(m_fSigma,m_fPixelStep,arr_GaussianWeights);
	//Loop each point
	for ( unsigned int iY = 0; iY < m_iHMDataHeight; iY++)
			for ( unsigned int iX = 0; iX < m_iHMDataWidth; iX++)
			{   
				unsigned int iCurPtIdx = iY*m_iHMDataWidth + iX;
				if(iCurPtIdx%(m_iHMDataWidth*10)==0)
				{
					strDebug.GenerateFormatSmallString("*");
					OutputDebugInfoThread(strDebug);
				}
				//ignore 'No-data' point case
				if(m_arrHeightMapData[iCurPtIdx] < 0)
				{
					NoDataPts_Arr.push_back(UnsolvedPoint(iX,iY));
					continue;
				}
				//1.compute the average position
				int iStep = m_iNormalWndSize/2;
				//the w channel for storing Gaussian weight
				std::vector<D3DXVECTOR4> LocalPts_Arr;
				float fTotalGaussianWeight = 0.0f;
				for (int j = -iStep; j <= iStep; j++)
					for(int i = -iStep; i <= iStep; i++)
					{
						int idx_x = i+iX;
						int idx_y = j+iY;
						if(idx_x < 0 || idx_y < 0 || idx_x >= (int)(m_iHMDataWidth) || idx_y >= (int)(m_iHMDataHeight))
							continue;

						unsigned int idx = idx_y*m_iHMDataWidth+idx_x;
						//'No-data' point case
						if(m_arrHeightMapData[idx] < 0)
							continue;

						unsigned int iGaussWeightIdx = (j+iStep)*m_iNormalWndSize + (i+iStep);
						float fGaussianWeight = arr_GaussianWeights[iGaussWeightIdx];
						LocalPts_Arr.push_back(D3DXVECTOR4(m_arrVertices[idx].Position,fGaussianWeight));
						fTotalGaussianWeight += fGaussianWeight;
					}
					//there is no enough valid points in neighborhood!
					size_t iNumLocalPts = LocalPts_Arr.size();
					if ((fTotalGaussianWeight < m_fGaussianWeightLimit) || (iNumLocalPts < 3) )
					{
						NoDataPts_Arr.push_back(UnsolvedPoint(iX,iY));
						continue;
					}

					//1.weighted average center
					D3DXVECTOR3 iCenterPos(0,0,0);
					for(unsigned int idx = 0; idx < iNumLocalPts; idx++)
					{
						////Renormalize weight values
						LocalPts_Arr[idx].w /= fTotalGaussianWeight;
						iCenterPos += D3DXVECTOR3(LocalPts_Arr[idx].x,LocalPts_Arr[idx].y,LocalPts_Arr[idx].z)*LocalPts_Arr[idx].w;	
					}

					//2.Construct convariance matrix M*M_T
					double** CovMat = new double*[4];
					double **V = new double*[4];
					double W[4];
					for(int i = 0; i < 4; i++)
					{
						CovMat[i] = new double[4];
						V[i] = new double[4];
					}

					std::vector<double> xvec;
					std::vector<double> yvec;
					std::vector<double> zvec;

					for(unsigned int i = 0; i < iNumLocalPts; i++)
					{
						float fNewWeight = LocalPts_Arr[i].w;
						xvec.push_back((LocalPts_Arr[i].x - iCenterPos.x)*fNewWeight);
						yvec.push_back((LocalPts_Arr[i].y - iCenterPos.y)*fNewWeight);
						zvec.push_back((LocalPts_Arr[i].z - iCenterPos.z)*fNewWeight);
					}

					//compute elements
					CovMat[1][1] = 0;
					for(unsigned int i =0; i < iNumLocalPts; i++)
						CovMat[1][1] += xvec[i] * xvec[i];

					CovMat[1][2] = 0;
					for(unsigned int i =0; i < iNumLocalPts; i++)
						CovMat[1][2] += xvec[i] * yvec[i];

					CovMat[1][3] = 0;
					for(unsigned int i =0; i < iNumLocalPts; i++)
						CovMat[1][3] += xvec[i] * zvec[i];

					CovMat[2][1] = CovMat[1][2];
					CovMat[2][2] = 0;
					for(unsigned int i =0; i < iNumLocalPts; i++)
						CovMat[2][2] += yvec[i] * yvec[i];

					CovMat[2][3] = 0;
					for(unsigned int i =0; i < iNumLocalPts; i++)
						CovMat[2][3] += yvec[i] * zvec[i];

					CovMat[3][1] = CovMat[1][3];
					CovMat[3][2] = CovMat[2][3];
					CovMat[3][3] = 0;
					for(unsigned int i =0; i < iNumLocalPts; i++)
						CovMat[3][3] += zvec[i] * zvec[i];

					if (!SVD::svdcmp(CovMat,3,3,W,V))
					{
#ifdef _DEBUG
						OUTPUTERRORINFO;
#endif
						return false;
					}

					int iMinIdx = -1;
					//int iMaxIdx = -1;
					double minVal = DBL_MAX;
					//double maxVal = DBL_MIN;
					for(int k = 1; k <4; k++)
					{
						if(W[k] < minVal)
						{
							minVal = W[k];
							iMinIdx = k;
						}
					}

					D3DXVECTOR3 norm = D3DXVECTOR3(V[1][iMinIdx],V[2][iMinIdx],V[3][iMinIdx]);
					if(norm.z < 0)
						norm *= -1.0f;

					//3. Fill the normal of current pt
					VertexNorm vert;
					vert.Position = m_arrVertices[iCurPtIdx].Position;
					D3DXVec3Normalize(&(vert.Normal),&norm);
					m_arrNormalAreaWeighted.push_back(vert);

					//release memory
					for (int i = 0; i < 4; i++)
					{
						SAFE_DELETE_ARRAY(CovMat[i]);
						SAFE_DELETE_ARRAY(V[i]);
					}
					SAFE_DELETE_ARRAY(CovMat);
					SAFE_DELETE_ARRAY(V);
			}

		strDebug.GenerateFormatSmallString("\n NumsolvedPt:%d, NumUnsolvedPt:%d\n",m_arrNormalAreaWeighted.size(),NoDataPts_Arr.size());
		OutputDebugInfoThread(strDebug);
		strDebug.GenerateFormatSmallString("\n Normal computing is finished!\n");
		OutputDebugInfoThread(strDebug);

		timer.Stop();
		strDebug.GenerateFormatSmallString("timing: %f ms\n", timer.GetDuration());
		OutputDebugInfoThread(strDebug);

		return true;
}

bool CXYZFileIO::EstimateNormalAreaWeighted_IgnoreNoDataPt()
{
	bool bRtVal = true;
	if(!m_bDataReady)
		return false;
	//Firstly fill the vertices array using positions
	if(m_arrVertices.size() != m_iHMDataWidth*m_iHMDataHeight)
	{
		m_arrVertices.clear();
		bRtVal = GenerateVerticesPos(-0.5f,-0.5f);
		if(!bRtVal)
			return bRtVal;
	}

	//find all the nodata point and its one-ring neighbors
	bool *bValidArr = new bool[m_iHMDataHeight*m_iHMDataWidth];
	memset(bValidArr,0,sizeof(bool)*m_iHMDataHeight*m_iHMDataWidth);//init to false
	for(unsigned int i = 0; i < m_arrNoDataPtIdx.size(); i++)
	{
		UnsolvedPoint PtIdx = m_arrNoDataPtIdx[i];
		unsigned int idx = PtIdx.iPtY*m_iHMDataWidth + PtIdx.iPtX;
		bValidArr[idx] = true;
		
		int iPtX1 = PtIdx.iPtX - 1;
		int iPtX2 = PtIdx.iPtX + 1;
		int iPtY1 = PtIdx.iPtY - 1;
		int iPtY2 = PtIdx.iPtY + 1;

		if(iPtX1 >= 0)
		{
			bValidArr[PtIdx.iPtY*m_iHMDataWidth + iPtX1] = true;
			if(iPtY1 >= 0)
				bValidArr[iPtY1*m_iHMDataWidth + iPtX1] = true;
			if(iPtY2 < (int)(m_iHMDataHeight))
				bValidArr[iPtY2*m_iHMDataWidth + iPtX1] = true;
		}

		if(iPtY1 >= 0)
		{
			bValidArr[iPtY1*m_iHMDataWidth + PtIdx.iPtX] = true;
			if(iPtX2 < (int)(m_iHMDataWidth))
				bValidArr[iPtY1*m_iHMDataWidth + iPtX2] = true;
		}

		if(iPtX2 < (int)(m_iHMDataWidth) )
		{
			bValidArr[PtIdx.iPtY*m_iHMDataWidth + iPtX2] = true;
			if(iPtY2 < (int)(m_iHMDataHeight))
				bValidArr[iPtY2*m_iHMDataWidth + iPtX2] = true;
		}

		if(iPtY2 < (int)(m_iHMDataHeight))
			bValidArr[iPtY2*m_iHMDataWidth + PtIdx.iPtX] = true;
	}
	
	//start to compute normal
	AreaWeightedVert* AreaWeightedVertArr = new AreaWeightedVert[m_iHMDataWidth*m_iHMDataHeight];
	//loop all the triangles
	UtilityString strDebug("Area-based normal computation starts:!\n");
	OutputDebugInfoThread(strDebug);
	for(unsigned int j = 0; j <(m_iHMDataHeight-1); j++)
		for(unsigned int i = 0; i <(m_iHMDataWidth-1); i++)
		{
			unsigned int idx1 = j*m_iHMDataWidth+i;
			unsigned int idx2 = idx1 + 1;
			unsigned int idx3 = idx1 + m_iHMDataWidth;
			unsigned int idx4 = idx2 + m_iHMDataWidth;
			bool bIdx1 = bValidArr[idx1];
			bool bIdx2 = bValidArr[idx2];
			bool bIdx3 = bValidArr[idx3];
			bool bIdx4 = bValidArr[idx4];
			if(bIdx1&&bIdx2&&bIdx3&&bIdx4)//==true --> ignore
				continue;
			if(idx1%(m_iHMDataWidth*10)==0) 
			{
				strDebug.GenerateFormatSmallString("*");
				OutputDebugInfoThread(strDebug);
			}

			D3DXVECTOR3 v1 = m_arrVertices[idx1].Position;
			D3DXVECTOR3 v2 = m_arrVertices[idx2].Position;
			D3DXVECTOR3 v3 = m_arrVertices[idx3].Position;
			D3DXVECTOR3 v4 = m_arrVertices[idx4].Position;
			//first triangle --> v1v2v4 -->(v2-v1)x(v4-v1)
			D3DXVECTOR3 normal1;
			D3DXVECTOR3 tmp;
			D3DXVECTOR3 v2v1 = v2-v1;
			D3DXVECTOR3 v4v1 = v4-v1;
			D3DXVec3Cross(&tmp,&v2v1,&v4v1);
			D3DXVec3Normalize(&normal1, &tmp);			
			float area1 = CD3DUtilityFunction::ComputeTriangleArea(v1,v2,v4);
			normal1*=area1;
			if(!bIdx1) {
				AreaWeightedVertArr[idx1].TotalNormal += normal1;
				AreaWeightedVertArr[idx1].TotalArea += area1;
			}
			if(!bIdx2) {
				AreaWeightedVertArr[idx2].TotalNormal += normal1;
				AreaWeightedVertArr[idx2].TotalArea += area1;
			}
			if(!bIdx4) {
				AreaWeightedVertArr[idx4].TotalNormal += normal1;
				AreaWeightedVertArr[idx4].TotalArea += area1;
			}
			//second triangle --> v1v4v3 --> (v4-v1)x(v3-v1)
			D3DXVECTOR3 normal2;
			D3DXVECTOR3 v3v1 = v3-v1;
			D3DXVec3Cross(&tmp,&v4v1,&v3v1);
			D3DXVec3Normalize(&normal2,&tmp);
			float area2 = CD3DUtilityFunction::ComputeTriangleArea(v1,v4,v3);
			normal2 *= area2;
			if(!bIdx1) {
				AreaWeightedVertArr[idx1].TotalNormal += normal2;
				AreaWeightedVertArr[idx1].TotalArea += area2;
			}
			if(!bIdx4) {
				AreaWeightedVertArr[idx4].TotalNormal += normal2;
				AreaWeightedVertArr[idx4].TotalArea += area2;
			}
			if(!bIdx3) {
				AreaWeightedVertArr[idx3].TotalNormal += normal2;
				AreaWeightedVertArr[idx3].TotalArea += area2;						
			}
		}

		m_arrNormalAreaWeighted.clear();
		for (unsigned int i = 0; i < m_arrVertices.size(); i++)
		{
			if(bValidArr[i])
				continue;
			D3DXVECTOR3 tmp = AreaWeightedVertArr[i].TotalNormal/AreaWeightedVertArr[i].TotalArea;
			D3DXVECTOR3 normal;
			D3DXVec3Normalize(&normal,&tmp);
#ifdef _DEBUG
			if(normal.z < 0)
				printf("neg");
#endif
			VertexNorm vert;
			vert.Position = m_arrVertices[i].Position;
			vert.Normal = normal;
			m_arrNormalAreaWeighted.push_back(vert);
		}

		//delete array
		SAFE_DELETE_ARRAY(AreaWeightedVertArr);
		SAFE_DELETE_ARRAY(bValidArr);
		strDebug.GenerateFormatSmallString("\n finished!\n");
		OutputDebugInfoThread(strDebug);
		return bRtVal;
}

bool CXYZFileIO::EstimateNormalAreaWeighted()
{
	bool bRtVal = true;
	if(!m_bDataReady)
		return false;
	//Firstly fill the vertices array using positions
	if(m_arrVertices.size() != m_iHMDataWidth*m_iHMDataHeight)
	{
		m_arrVertices.clear();
		bRtVal = GenerateVerticesPos(-0.5f,-0.5f);
	}

	//start to compute normal
	AreaWeightedVert* AreaWeightedVertArr = new AreaWeightedVert[m_iHMDataWidth*m_iHMDataHeight];
	//loop all the triangles
	UtilityString strDebug("Area-based normal computation starts:!\n");
	OutputDebugInfoThread(strDebug);

	for(unsigned int j = 0; j <(m_iHMDataHeight-1); j++)
		for(unsigned int i = 0; i <(m_iHMDataWidth-1); i++)
		{
			unsigned int idx1 = j*m_iHMDataWidth+i;
			unsigned int idx2 = idx1 + 1;
			unsigned int idx3 = idx1 + m_iHMDataWidth;
			unsigned int idx4 = idx2 + m_iHMDataWidth;

			if(idx1%(m_iHMDataWidth*10)==0) 
			{
				strDebug.GenerateFormatSmallString("*");
			    OutputDebugInfoThread(strDebug);
			}

			D3DXVECTOR3 v1 = m_arrVertices[idx1].Position;
			D3DXVECTOR3 v2 = m_arrVertices[idx2].Position;
			D3DXVECTOR3 v3 = m_arrVertices[idx3].Position;
			D3DXVECTOR3 v4 = m_arrVertices[idx4].Position;
			//first triangle --> v1v2v4 -->(v2-v1)x(v4-v1)
			D3DXVECTOR3 normal1;
			D3DXVECTOR3 tmp;
			D3DXVECTOR3 v2v1 = v2-v1;
			D3DXVECTOR3 v4v1 = v4-v1;
			D3DXVec3Cross(&tmp,&v2v1,&v4v1);
			D3DXVec3Normalize(&normal1, &tmp);			
			float area1 = CD3DUtilityFunction::ComputeTriangleArea(v1,v2,v4);
			normal1*=area1;
			AreaWeightedVertArr[idx1].TotalNormal += normal1;
			AreaWeightedVertArr[idx1].TotalArea += area1;
			AreaWeightedVertArr[idx2].TotalNormal += normal1;
			AreaWeightedVertArr[idx2].TotalArea += area1;
			AreaWeightedVertArr[idx4].TotalNormal += normal1;
			AreaWeightedVertArr[idx4].TotalArea += area1;
			
			//second triangle --> v1v4v3 --> (v4-v1)x(v3-v1)
			D3DXVECTOR3 normal2;
			D3DXVECTOR3 v3v1 = v3-v1;
			D3DXVec3Cross(&tmp,&v4v1,&v3v1);
			D3DXVec3Normalize(&normal2,&tmp);
			float area2 = CD3DUtilityFunction::ComputeTriangleArea(v1,v4,v3);
			normal2 *= area2;
			AreaWeightedVertArr[idx1].TotalNormal += normal2;
			AreaWeightedVertArr[idx1].TotalArea += area2;
			AreaWeightedVertArr[idx4].TotalNormal += normal2;
			AreaWeightedVertArr[idx4].TotalArea += area2;
			AreaWeightedVertArr[idx3].TotalNormal += normal2;
			AreaWeightedVertArr[idx3].TotalArea += area2;						
			
		}

		m_arrNormalAreaWeighted.clear();
		for (unsigned int i = 0; i < m_arrVertices.size(); i++)
		{
			D3DXVECTOR3 tmp = AreaWeightedVertArr[i].TotalNormal/AreaWeightedVertArr[i].TotalArea;
			D3DXVECTOR3 normal;
			D3DXVec3Normalize(&normal,&tmp);
#ifdef _DEBUG
			if(normal.z < 0)
				printf("neg");
#endif
			VertexNorm vert;
			vert.Position = m_arrVertices[i].Position;
			vert.Normal = normal;
			m_arrNormalAreaWeighted.push_back(vert);
		}

		//delete array
		SAFE_DELETE_ARRAY(AreaWeightedVertArr);
		strDebug.GenerateFormatSmallString("\nfinished!\n");
		OutputDebugInfoThread(strDebug);

		return bRtVal;
}

void CXYZFileIO::SetStrXYZFilePathName(const char* strFN)
{
	ZeroMemory(m_strXYZFileName,sizeof(char)*MAX_PATH);
	sprintf_s(m_strXYZFileName,strFN);
}

HRESULT CXYZFileIO::Save2DNDFImg()
{
	HRESULT hr = S_OK;

	//Generate 2D normal histogram image
	CString strNHImgFullPath(m_strXYZFileName);
	int iPos = strNHImgFullPath.ReverseFind('\\');
	CString strNHImgName = strNHImgFullPath.Mid(iPos+1);
	strNHImgFullPath.TrimRight(strNHImgName);
	int iPos1 = strNHImgName.ReverseFind('.');
	CString strTmpExt = strNHImgName.Mid(iPos1);
	strNHImgName.TrimRight(strTmpExt);
	strNHImgName.Append(_T("_2DNDF.bmp"));
	strNHImgFullPath.Append(strNHImgName);
	//SAVE BMP 
	ID3D11Device* pd3dDevice = AfxDXGlobal()->Device();

	//hr = CD3DUtilityFunction::SaveNDFImage_Gray(pd3dDevice,strNHImgFullPath,m_i2DNDFImgDim,m_i2DNDFImgDim,m_arrNDFCumulation);
	hr = CD3DUtilityFunction::SaveNDFImage(pd3dDevice,strNHImgFullPath,m_i2DNDFImgDim,m_i2DNDFImgDim,m_arrNDFCumulation);
	//SAVE DDS file
	strNHImgFullPath.TrimRight(_T(".bmp"));
	strNHImgFullPath.Append(_T(".dds"));
	hr = CD3DUtilityFunction::SaveNDFFloatImageData(pd3dDevice,strNHImgFullPath,m_i2DNDFImgDim,m_i2DNDFImgDim,m_arrNDFCumulation);

	if(hr == S_OK) 
	{
		CString strTmp;
		strTmp.Format(_T("%s Generated!"), strNHImgFullPath);
		theApp.OutputString(strTmp);
	}
	return hr;
}

void CXYZFileIO::ComputeNDFCumulationArray()
{
	if(m_arrNormalAreaWeighted.size() == 0)
		return;

	m_arrNDFCumulation.clear();

	for (unsigned int j = 0; j < m_i2DNDFImgDim; j++)
		for (unsigned int i = 0; i < m_i2DNDFImgDim; i++)
			m_arrNDFCumulation.push_back(0);	
	//loop all normals
	for (size_t i = 0; i < m_arrNormalAreaWeighted.size(); i++)
	{
		//D3DXVECTOR3 norm = m_arrNormalAreaWeighted[i].Normal;
		D3DXVECTOR3 norm;
		D3DXVec3TransformNormal(&norm,&(m_arrNormalAreaWeighted[i].Normal),&m_matRotNormal);

		norm.x = norm.x/2.0f + 0.5f; //(-1,1) --> (0,1); 
		norm.y = norm.y/2.0f + 0.5f;
		//UtilityString strDebug;
		//strDebug.GenerateFormatSmallString("(%f,%f,%f)", norm.x, norm.y, norm.z);
		//OutputDebugInfoThread(strDebug);
		int iPosX = (int)(floorf(norm.x * m_i2DNDFImgDim));
		int iPosY = (int)(floorf(norm.y * m_i2DNDFImgDim));
		if(iPosX == m_i2DNDFImgDim) iPosX--;
		if(iPosY == m_i2DNDFImgDim) iPosY--;
		unsigned int idx = iPosY*m_i2DNDFImgDim + iPosX;
		m_arrNDFCumulation[idx] += 1.0;
	}

#if 1
	FILE* fpNorm = NULL;
	fopen_s(&fpNorm, "accumNorm.txt", "w");
	for (unsigned int j = 0; j < m_i2DNDFImgDim; j++){
		for(unsigned int i = 0; i < m_i2DNDFImgDim; i++){
			unsigned int idx = j*m_i2DNDFImgDim + i;
			fprintf(fpNorm,"%f ", m_arrNDFCumulation[idx]);
		}
		fprintf(fpNorm,"\n");	
	}
	fclose(fpNorm);

	double fMaxVal = FLT_MIN;
	double fSumNumNormals = 0;
	for (unsigned int j = 0; j < m_i2DNDFImgDim; j++)
		for(unsigned int i = 0; i < m_i2DNDFImgDim; i++)
		{
			//printf("%f\t", pAccumBuf[j*iWidth+i]);
			if(m_arrNDFCumulation[j*m_i2DNDFImgDim+i] > fMaxVal)
				fMaxVal = m_arrNDFCumulation[j*m_i2DNDFImgDim+i];
			fSumNumNormals += m_arrNDFCumulation[j*m_i2DNDFImgDim+i];
		}

		UtilityString strDebug;
		strDebug.GenerateFormatSmallString("fMaxVal=%f", fMaxVal);
		OutputDebugInfoThread(strDebug);

		strDebug.GenerateFormatSmallString("cornerVal1=%f", m_arrNDFCumulation[(m_i2DNDFImgDim-1)*m_i2DNDFImgDim+(m_i2DNDFImgDim/2)]);
		OutputDebugInfoThread(strDebug);
		strDebug.GenerateFormatSmallString("cornerVal2=%f", m_arrNDFCumulation[(m_i2DNDFImgDim/2)]);
		OutputDebugInfoThread(strDebug);
		strDebug.GenerateFormatSmallString("cornerVal3=%f", m_arrNDFCumulation[(m_i2DNDFImgDim/2-1)*m_i2DNDFImgDim]);
		OutputDebugInfoThread(strDebug);
		strDebug.GenerateFormatSmallString("cornerVal3=%f", m_arrNDFCumulation[(m_i2DNDFImgDim/2-1)*m_i2DNDFImgDim+(m_i2DNDFImgDim-1)]);
		OutputDebugInfoThread(strDebug);

#endif
	double fNormalizeFactor = 0;
	double fProjectSAPerPixel = (4.0)/(m_i2DNDFImgDim*m_i2DNDFImgDim);
	fNormalizeFactor = fSumNumNormals*fProjectSAPerPixel;
	strDebug.GenerateFormatSmallString("fNormalizeFactor=%f", fNormalizeFactor);
	OutputDebugInfoThread(strDebug);

		//normalization
		//and save matlab fitting data (.mfd)
		CString strMFDFullPath(m_strXYZFileName);
		int iPos = strMFDFullPath.ReverseFind('\\');
		CString strMFDName = strMFDFullPath.Mid(iPos+1);
		strMFDFullPath.TrimRight(strMFDName);
		CString strTableFilePath = strMFDFullPath;
		int iPos1 = strMFDName.ReverseFind('.');
		CString strTmpExt = strMFDName.Mid(iPos1);
		strMFDName.TrimRight(strTmpExt);
		CString strTableFile = strMFDName;
		strMFDName.Append(_T(".mfd"));
		strMFDFullPath.Append(strMFDName);
		strTableFile.Append(_T(".txt"));
		strTableFilePath.Append(strTableFile);
	

		FILE* fp = _tfopen((LPCTSTR)strMFDFullPath,_T("w"));
		FILE* fp1 = _tfopen((LPCTSTR)strTableFilePath,_T("w"));
		fwprintf(fp1,_T("%d %d\n"),m_i2DNDFImgDim,m_i2DNDFImgDim);

		for (unsigned int j = 0; j < m_i2DNDFImgDim; j++)
			for(unsigned int i = 0; i < m_i2DNDFImgDim; i++)
			{
				int idx = j*m_i2DNDFImgDim+i;
				//m_arrNDFCumulation[idx] /= fMaxVal;
				m_arrNDFCumulation[idx] /= fNormalizeFactor;
				//m_arrNDFCumulation[idx] /= m_arrNormalAreaWeighted.size();
				//save .mfd file
				if(m_arrNDFCumulation[idx] > m_fGenMFDThreshold)
				{
					float fThetaM = 0.0f, fPhiM = 0.0f;
					CD3DUtilityFunction::Convert2DPosTo2Angles(i,j,m_i2DNDFImgDim,m_i2DNDFImgDim,fThetaM,fPhiM);
					fwprintf(fp,_T("%f %f %f\n"),fThetaM,fPhiM,m_arrNDFCumulation[idx]);
					fwprintf(fp1,_T("%f "),m_arrNDFCumulation[idx]);
				}
				else
					fwprintf(fp1,_T("0 "));
				//
				if(i == m_i2DNDFImgDim-1)
					fwprintf(fp1,_T("\n"));
			}
		fclose(fp);
		fclose(fp1);
}

bool CXYZFileIO::computeWaveOpticBRDF_MultipleFiles(CString& strFNs, float fSampleWidth, float fSampleHeight)
{
	CString strSelectedFiles(strFNs);
	strSelectedFiles.TrimLeft();
	strSelectedFiles.TrimRight();

	int iPos = strSelectedFiles.ReverseFind(_T('\\'));
	CString strFileNames = strSelectedFiles.Mid(iPos+1);
	CString strFileFolderPath = strSelectedFiles.TrimRight(strFileNames);
	strFileNames.TrimLeft();strFileNames.TrimRight();

	bool bFinished = false;
	UtilityString strDebug("Start Compute Wave Optic BRDF from multiple files:");
	OutputDebugInfoThread(strDebug);

	//save the wBRDF data
	OPENFILENAME spf;
	ZeroMemory( &spf, sizeof(spf));
	//maximumlly 10 files
	const DWORD iMaxNumFiles = 1;
	const DWORD fileNameMaxLength = MAX_PATH + 1;
	const DWORD bufferSize = (iMaxNumFiles * fileNameMaxLength) + 1;
	TCHAR* strFileName = new TCHAR[bufferSize];
	memset(strFileName,0,sizeof(TCHAR)*bufferSize);

	spf.hwndOwner = 0;
	spf.lpstrFilter = L"LOG Files (*.log)\0*.log\0All Files (*.*)\0*.*\0";
	spf.lpstrCustomFilter = 0;
	spf.nMaxCustFilter = 0L;
	spf.nFilterIndex = 1L;
	spf.lpstrFile = strFileName;
	spf.nMaxFile = bufferSize;
	spf.lpstrFileTitle = 0;
	spf.nMaxFileTitle= 0;
	spf.lpstrInitialDir = (LPCTSTR)strFileFolderPath;
	spf.lpstrTitle = L"Open .log Files";
	spf.nFileOffset = 0;
	spf.nFileExtension = 0;
	spf.lpstrDefExt = L"*.log";
	spf.lpfnHook = NULL;
	spf.lCustData = 0;
	spf.Flags = OFN_PATHMUSTEXIST | OFN_OVERWRITEPROMPT | OFN_EXPLORER | OFN_HIDEREADONLY;
	spf.lStructSize = sizeof(OPENFILENAME);

	bool bSaveLog = false;
	FILE* fp_Log = NULL;
	if(GetSaveFileName(&spf)) {
		_tfopen_s(&fp_Log,(const TCHAR*)(spf.lpstrFile), L"w");
		UtilityString strDebug1;
		strDebug1.SetWCharStr(spf.lpstrFile);
		OutputDebugInfoThread(strDebug1);
		bSaveLog = true;
	}

	//clear array
	m_arrWaveOpticBRDF_MFiles.clear();

	bool bFristFile = true;
	int iNumFiles = 0;
	while (!bFinished)
	{
		CString strSingleFN;
		int iPos1 = strFileNames.Find(_T(' '));
		if(iPos1 >= 0)
			strSingleFN = strFileNames.Left(iPos1);
		else
		{
			strSingleFN = strFileNames;
			bFinished = true;
		}
		strFileNames.TrimLeft(strSingleFN);
		strFileNames.TrimLeft();
		CString strSingleFileFullPath = strFileFolderPath + strSingleFN;

		//theApp.OutputString(strSingleFileFullPath);
		//process each file!
		CXYZFileIO* pSingleXYZFile = new CXYZFileIO();

		float fCurSampleWidth = fSampleWidth;
		float fCurSampleHeight = fSampleHeight;
		//load files
		if(pSingleXYZFile->LoadFromFile(CT2A(strSingleFileFullPath),fCurSampleWidth,fCurSampleHeight) < 0)
			return false;
		//generate vertices for xyz file
		pSingleXYZFile->m_arrVertices.clear();
		bool bRtVal = pSingleXYZFile->GenerateVerticesPos(-0.5f,-0.5f);
		if(!bRtVal)
			return bRtVal;
	
		//compute wave optic brdf
		pSingleXYZFile->SetCurGonioDir(m_iGonioAxis);
		pSingleXYZFile->computeWaveOpticBRDF();
		if(bFristFile){
			for(size_t iWBrdf = 0; iWBrdf < pSingleXYZFile->m_arrWaveOpticBRDF.size(); iWBrdf++)
					m_arrWaveOpticBRDF_MFiles.push_back(pSingleXYZFile->m_arrWaveOpticBRDF[iWBrdf]);
			bFristFile = false;
			}else{
				//accumulate wave optic brdf
				for(size_t iWBrdf = 0; iWBrdf < pSingleXYZFile->m_arrWaveOpticBRDF.size(); iWBrdf++)
					m_arrWaveOpticBRDF_MFiles[iWBrdf].BRDFVal += pSingleXYZFile->m_arrWaveOpticBRDF[iWBrdf].BRDFVal;
			}		
		//delete
		SAFE_DELETE(pSingleXYZFile);
		UtilityString strTmp(strSingleFileFullPath.GetBuffer());
		strDebug.GenerateFormatSmallString("%s processed!", strTmp.GetCharStr());
		OutputDebugInfoThread(strDebug);

		//Save log file
		if(bSaveLog)
			_ftprintf(fp_Log,L"%s\n",strSingleFileFullPath.GetBuffer());
		//Increase the number of files
		iNumFiles++;
	}
	//average the BRDF values
	for(size_t iWBrdf = 0; iWBrdf < m_arrWaveOpticBRDF_MFiles.size(); iWBrdf++)
		m_arrWaveOpticBRDF_MFiles[iWBrdf].BRDFVal /= static_cast<double>(iNumFiles);
	
	//close log
	if(bSaveLog)
		fclose(fp_Log);
	return true;
}

bool CXYZFileIO::computeWaveOpticBRDF()
{
	//m_iGonioAxis = MINORAXIS;
	//read the gonio data file
	if( !PrepareGonioData() )
		return false;

	//clean buffer
	m_arrWaveOpticBRDF.clear();
	float radianperdegree = D3DX_PI/180.0f;
	//compute the wave optic brdf
	D3DXVECTOR3 vSurNorm(0,0,1);
	////conver from nm to um
	double fWavelength = (double)(m_iGonioWBand)/1000.0;

	for (int iHalfAng = 0; iHalfAng < m_GonioData.m_arrBRDFData.GetSize(); iHalfAng++){
		PCGGonioMeasuredData data = m_GonioData.m_arrBRDFData[iHalfAng];
		
		float inTheta = data.m_fThetaIn*radianperdegree;
		//problematic???
		float inPhi = 0.5*D3DX_PI;
		float outTheta = data.m_fThetaOut*radianperdegree;
		//problematic???
		float outPhi = 0.5*D3DX_PI;
		
		//problematic???
		if(m_iGonioAxis == MINORAXIS){
			inPhi = 0;
			outPhi = 0;
		}

		D3DXVECTOR3 vInVec, vTmp;
		vTmp.x = cos(inPhi)*sin(inTheta);
		vTmp.y = sin(inPhi)*sin(inTheta);
		vTmp.z = cos(inTheta);
		D3DXVec3Normalize(&vInVec,&vTmp);

		D3DXVECTOR3 vOutVec;
		vTmp.x = cos(outPhi)*sin(outTheta);
		vTmp.y = sin(outPhi)*sin(outTheta);
		vTmp.z = cos(outTheta);
		D3DXVec3Normalize(&vOutVec,&vTmp);

		WaveOpticBRDF wBRDF;
		wBRDF.vHalf_UnNorm = vInVec + vOutVec;
		wBRDF.BRDFVal = computeBRDFWaveOptics(vInVec, vOutVec, vSurNorm, fWavelength);
		wBRDF.m_fThetaM = (data.m_fThetaIn + data.m_fThetaOut)*0.5f;
		//Debug output
		UtilityString strDebug;
		strDebug.GenerateFormatSmallString("HalfAng:%f,BRDF:%f.",wBRDF.m_fThetaM,wBRDF.BRDFVal);
		OutputDebugInfoThread(strDebug);
		//add to array
		m_arrWaveOpticBRDF.push_back(wBRDF);
	}
	return true;
}

bool CXYZFileIO::PrepareGonioData()
{
	//prepare path
	UtilityString strGonioFilePath(m_strXYZFileName);	
	
	int iNum = 3;
	int iIdx = strGonioFilePath.SearchSubStringFirstOccurrence("_Split");
	if(iIdx >= 0) 
		iNum++;
	
	for(int i = 0; i < iNum; i++)
		strGonioFilePath.Remove1LevelSubDirectory();
	
	strGonioFilePath.ConcatenateStr("\\GonioData");
	switch (m_iGonioAxis)
	{
	case MAJORAXIS:
		strGonioFilePath.ConcatenateStr("\\MajorAxis\\MajorAxis_");
		break;

	case MINORAXIS:
		strGonioFilePath.ConcatenateStr("\\MinorAxis\\MinorAxis_");
		break;
	}
	
	UtilityString strGonioFileName;
	strGonioFileName.GenerateFormatSmallString("%dnm.txt", m_iGonioWBand);
	
	strGonioFilePath.ConcatenateUtilityStr(strGonioFileName);
	//PCG
	m_GonioData.ReadGonioFile(strGonioFilePath.GetCharStr(),m_iGonioWBand);
	m_GonioData.GenerateComparison(m_i2DNDFImgDim,m_arrGonioNDFData);

	//RIT
	m_GonioData_rit.ReadAngleMappingFile("D:\\MFCD3D11Test\\MFCD3D11Test\\OrgData\\Gonio_RIT\\AngleMapping.csv");
	if(m_iGonioAxis == MAJORAXIS) 
		m_GonioData_rit.ReadOrgGonioMeasuredData("D:\\MFCD3D11Test\\MFCD3D11Test\\OrgData\\Gonio_RIT\\CornellSteel_flop_0.Csv");
	else if (m_iGonioAxis == MINORAXIS)
		m_GonioData_rit.ReadOrgGonioMeasuredData("D:\\MFCD3D11Test\\MFCD3D11Test\\OrgData\\Gonio_RIT\\CornellSteel_90deg_flop_0.Csv",true,90);

	m_GonioData_rit.GenerateComparison(m_i2DNDFImgDim, m_arrGonioNDFData_RIT);

	return true;

}

/******************************************************************************************************/
/**Be careful: the wavelength value and the point position should be using the same unit, like nm or um**/
/******************************************************************************************************/
double CXYZFileIO::computeWaveOpticsTerm(const D3DXVECTOR3& vSurfNormal, const D3DXVECTOR3& vHalf_UnNorm, const double fWavelength)
{
	double kappa = 2*D3DX_PI/(fWavelength);
	double realPart = 0; 
	double imgPart = 0;

	if(m_arrVertices.size() == 0)
		return -1;

	int iNumPts = 0;
	int iNoDataPts = 0;
	for (size_t iPt = 0; iPt < m_arrVertices.size(); iPt++)	{
		VertexNorm vPt = m_arrVertices[iPt];
		//ignore no data point
		if(vPt.Position.z < 0){
			iNoDataPts++;
			continue;
		}
		//e^(i*(-1.0*kappa*dot(h,vPos)))
		//e^(ix) = cos(x)+i*sin(x)
		double dotVal = static_cast<double>(D3DXVec3Dot(&vHalf_UnNorm, &(vPt.Position)));
		double xVal = -1.0*kappa*dotVal;
		realPart += cos(xVal);
		imgPart += sin(xVal);
		iNumPts++;
	}

	UtilityString strDebug;
	strDebug.GenerateFormatSmallString("iNumPts=%d,iNoDataPts=%d.",iNumPts,iNoDataPts);
	OutputDebugInfoThread(strDebug);

	double scaleFactor = ( D3DXVec3LengthSq(&vHalf_UnNorm) ) / ( D3DXVec3Dot(&vHalf_UnNorm,&vSurfNormal)*iNumPts );
	realPart *= scaleFactor;
	imgPart *= scaleFactor;

	return (realPart*realPart + imgPart*imgPart);
}

double CXYZFileIO::computeBRDFWaveOptics(const D3DXVECTOR3& vLight, const D3DXVECTOR3& vView, 
	const D3DXVECTOR3& vSurfNormal, const double fWavelength)
{
	D3DXVECTOR3 vecL;
	D3DXVec3Normalize(&vecL,&vLight);
	D3DXVECTOR3 vecV;
	D3DXVec3Normalize(&vecV,&vView);

	//unnormalized half vector
	D3DXVECTOR3 vHalf_UnNorm = vecL + vecV;

	double Dw = computeWaveOpticsTerm(vSurfNormal, vHalf_UnNorm, fWavelength);
	if(Dw < 0)
		return -1;
	double denorm = 4.0*static_cast<double>( (D3DXVec3Dot(&vecV,&vSurfNormal))*(D3DXVec3Dot(&vecL,&vSurfNormal)) );
	double BRDFVal = Dw/denorm;

	return BRDFVal;		
}